OP-936: Merges branch 'next' into hyper/OP-936_task-monitor-rework, fixes damage/conflicts,

and brings the callback scheduler into the fold.

+review OPReview-461

Makefile

@@ -261,9 +261,9 @@ fw_$(1)_%: uavobjects_flight
 	$(V1) $(MKDIR) -p $(BUILD_DIR)/fw_$(1)/dep
 	$(V1) cd $(ROOT_DIR)/flight/targets/boards/$(1)/firmware && \
 		$$(MAKE) -r --no-print-directory \
+		BUILD_TYPE=fw \
 		BOARD_NAME=$(1) \
 		BOARD_SHORT_NAME=$(2) \
-		BUILD_TYPE=fw \
 		TOPDIR=$(ROOT_DIR)/flight/targets/boards/$(1)/firmware \
 		OUTDIR=$(BUILD_DIR)/fw_$(1) \
 		TARGET=fw_$(1) \
@@ -288,9 +288,9 @@ bl_$(1)_%:
 	$(V1) $(MKDIR) -p $(BUILD_DIR)/bl_$(1)/dep
 	$(V1) cd $(ROOT_DIR)/flight/targets/boards/$(1)/bootloader && \
 		$$(MAKE) -r --no-print-directory \
+		BUILD_TYPE=bl \
 		BOARD_NAME=$(1) \
 		BOARD_SHORT_NAME=$(2) \
-		BUILD_TYPE=bl \
 		TOPDIR=$(ROOT_DIR)/flight/targets/boards/$(1)/bootloader \
 		OUTDIR=$(BUILD_DIR)/bl_$(1) \
 		TARGET=bl_$(1) \
@@ -326,9 +326,9 @@ bu_$(1)_%: bl_$(1)_bino
 	$(V1) $(MKDIR) -p $(BUILD_DIR)/bu_$(1)/dep
 	$(V1) cd $(ROOT_DIR)/flight/targets/common/bootloader_updater && \
 		$$(MAKE) -r --no-print-directory \
+		BUILD_TYPE=bu \
 		BOARD_NAME=$(1) \
 		BOARD_SHORT_NAME=$(2) \
-		BUILD_TYPE=bu \
 		TOPDIR=$(ROOT_DIR)/flight/targets/common/bootloader_updater \
 		OUTDIR=$(BUILD_DIR)/bu_$(1) \
 		TARGET=bu_$(1) \
@@ -350,9 +350,9 @@ ef_$(1)_%: bl_$(1)_bin fw_$(1)_opfw
 	$(V1) $(MKDIR) -p $(BUILD_DIR)/ef_$(1)
 	$(V1) cd $(ROOT_DIR)/flight/targets/common/entire_flash && \
 		$$(MAKE) -r --no-print-directory \
+		BUILD_TYPE=ef \
 		BOARD_NAME=$(1) \
 		BOARD_SHORT_NAME=$(2) \
-		BUILD_TYPE=ef \
 		DFU_CMD="$(DFUUTIL_DIR)/bin/dfu-util" \
 		TOPDIR=$(ROOT_DIR)/flight/targets/common/entire_flash \
 		OUTDIR=$(BUILD_DIR)/ef_$(1) \
@@ -656,20 +656,9 @@ ut_$(1)_%: $$(UT_OUT_DIR)
 	$(V1) cd $(ROOT_DIR)/flight/tests/$(1) && \
 		$$(MAKE) -r --no-print-directory \
 		BUILD_TYPE=ut \
-		BOARD_SHORT_NAME=$(1) \
+		TOPDIR=$(ROOT_DIR)/flight/tests/$(1) \
-		TCHAIN_PREFIX="" \
-		REMOVE_CMD="$(RM)" \
-		\
-		TARGET=$(1) \
 		OUTDIR="$(UT_OUT_DIR)/$(1)" \
-		\
+		TARGET=$(1) \
-		PIOS=$(PIOS) \
-		OPUAVOBJ=$(OPUAVOBJ) \
-		OPUAVTALK=$(OPUAVTALK) \
-		FLIGHTLIB=$(FLIGHTLIB) \
-		\
-		GTEST_DIR=$(GTEST_DIR) \
-		\
 		$$*
 
 .PHONY: ut_$(1)_clean
@@ -847,6 +836,7 @@ docs_all_clean:
 
 .PHONY: build-info
 build-info:
+	@$(ECHO) " BUILD-INFO $(call toprel, $(BUILD_DIR)/$@.txt)"
 	$(V1) $(MKDIR) -p $(BUILD_DIR)
 	$(V1) $(VERSION_INFO) \
 		--uavodir=$(ROOT_DIR)/shared/uavobjectdefinition \
@@ -865,7 +855,8 @@ build-info:
 help:
 	@$(ECHO)
 	@$(ECHO) "   This Makefile is known to work on Linux and Mac in a standard shell environment."
-	@$(ECHO) "   It also works on Windows by following the instructions in make/winx86/README.txt."
+	@$(ECHO) "   It also works on Windows by following the instructions given on this wiki page:"
+	@$(ECHO) "       http://wiki.openpilot.org/display/Doc/Windows%3A+Building+and+Packaging"
 	@$(ECHO)
 	@$(ECHO) "   Here is a summary of the available targets:"
 	@$(ECHO)
@@ -874,12 +865,18 @@ help:
 	@$(ECHO) "     qt_sdk_install       - Install the QT development tools"
 	@$(ECHO) "     mingw_install        - Install the MinGW toolchain (Windows only)"
 	@$(ECHO) "     python_install       - Install the Python interpreter (Windows only)"
+	@$(ECHO) "     nsis_install         - Install the NSIS Unicode (Windows only)"
 	@$(ECHO) "     uncrustify_install   - Install the Uncrustify source code beautifier"
+	@$(ECHO) "     doxygen_install      - Install the Doxygen documentation generator"
+	@$(ECHO) "     gtest_install        - Install the GoogleTest framework"
+	@$(ECHO) "   These targets are not updated yet and are probably broken:"
 	@$(ECHO) "     openocd_install      - Install the OpenOCD JTAG daemon"
 	@$(ECHO) "     stm32flash_install   - Install the stm32flash tool for unbricking F1-based boards"
 	@$(ECHO) "     dfuutil_install      - Install the dfu-util tool for unbricking F4-based boards"
 	@$(ECHO) "     android_sdk_install  - Install the Android SDK tools"
+	@$(ECHO) "   Install all available tools:"
 	@$(ECHO) "     all_sdk_install      - Install all of above (platform-dependent)"
+	@$(ECHO) "     build_sdk_install    - Install only essential for build tools (platform-dependent)"
 	@$(ECHO)
 	@$(ECHO) "   Other tool options are:"
 	@$(ECHO) "     <tool>_version       - Display <tool> version"
@@ -908,32 +905,32 @@ help:
 	@$(ECHO)
 	@$(ECHO) "   [Firmware]"
 	@$(ECHO) "     <board>              - Build firmware for <board>"
-	@$(ECHO) "                            supported boards are ($(ALL_BOARDS))"
+	@$(ECHO) "                            Supported boards are ($(ALL_BOARDS))"
 	@$(ECHO) "     fw_<board>           - Build firmware for <board>"
-	@$(ECHO) "                            supported boards are ($(FW_BOARDS))"
+	@$(ECHO) "                            Supported boards are ($(FW_BOARDS))"
 	@$(ECHO) "     fw_<board>_clean     - Remove firmware for <board>"
 	@$(ECHO) "     fw_<board>_program   - Use OpenOCD + JTAG to write firmware to <board>"
 	@$(ECHO)
 	@$(ECHO) "   [Bootloader]"
 	@$(ECHO) "     bl_<board>           - Build bootloader for <board>"
-	@$(ECHO) "                            supported boards are ($(BL_BOARDS))"
+	@$(ECHO) "                            Supported boards are ($(BL_BOARDS))"
 	@$(ECHO) "     bl_<board>_clean     - Remove bootloader for <board>"
 	@$(ECHO) "     bl_<board>_program   - Use OpenOCD + JTAG to write bootloader to <board>"
 	@$(ECHO)
 	@$(ECHO) "   [Entire Flash]"
 	@$(ECHO) "     ef_<board>           - Build entire flash image for <board>"
-	@$(ECHO) "                            supported boards are ($(EF_BOARDS))"
+	@$(ECHO) "                            Supported boards are ($(EF_BOARDS))"
 	@$(ECHO) "     ef_<board>_clean     - Remove entire flash image for <board>"
 	@$(ECHO) "     ef_<board>_program   - Use OpenOCD + JTAG to write entire flash image to <board>"
 	@$(ECHO)
 	@$(ECHO) "   [Bootloader Updater]"
 	@$(ECHO) "     bu_<board>           - Build bootloader updater for <board>"
-	@$(ECHO) "                            supported boards are ($(BU_BOARDS))"
+	@$(ECHO) "                            Supported boards are ($(BU_BOARDS))"
 	@$(ECHO) "     bu_<board>_clean     - Remove bootloader updater for <board>"
 	@$(ECHO)
 	@$(ECHO) "   [Unbrick a board]"
 	@$(ECHO) "     unbrick_<board>      - Use the STM32's built in boot ROM to write a bootloader to <board>"
-	@$(ECHO) "                            supported boards are ($(BL_BOARDS))"
+	@$(ECHO) "                            Supported boards are ($(BL_BOARDS))"
 	@$(ECHO) "   [Unittests]"
 	@$(ECHO) "     ut_<test>            - Build unit test <test>"
 	@$(ECHO) "     ut_<test>_xml        - Run test and capture XML output into a file"
@@ -942,14 +939,14 @@ help:
 	@$(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) "     sim_win32            - Build OpenPilot simulation firmware for Windows"
-	@$(ECHO) "                            Windows using mingw and msys"
+	@$(ECHO) "                            using mingw and msys"
 	@$(ECHO) "     sim_win32_clean      - Delete all build output for the win32 simulation"
 	@$(ECHO)
 	@$(ECHO) "   [GCS]"
 	@$(ECHO) "     gcs                  - Build the Ground Control System (GCS) application (debug|release)"
 	@$(ECHO) "     gcs_clean            - Remove the Ground Control System (GCS) application (debug|release)"
-	@$(ECHO) "                            supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
+	@$(ECHO) "                            Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
 	@$(ECHO) "     gcs_all_clean        - Remove the Ground Control System (GCS) application (all build confgurations)"
 	@$(ECHO)
 	@$(ECHO) "   [AndroidGCS]"
@@ -960,22 +957,35 @@ help:
 	@$(ECHO)
 	@$(ECHO) "   [UAVObjects]"
 	@$(ECHO) "     uavobjects           - Generate source files from the UAVObject definition XML files"
-	@$(ECHO) "     uavobjects_test      - parse xml-files - check for valid, duplicate ObjId's, ... "
+	@$(ECHO) "     uavobjects_test      - Parse xml-files - check for valid, duplicate ObjId's, ..."
 	@$(ECHO) "     uavobjects_<group>   - Generate source files from a subset of the UAVObject definition XML files"
-	@$(ECHO) "                            supported groups are ($(UAVOBJ_TARGETS))"
+	@$(ECHO) "                            Supported groups are ($(UAVOBJ_TARGETS))"
 	@$(ECHO)
 	@$(ECHO) "   [Packaging]"
-	@$(ECHO) "     opfw_resource        - Generate resources to embed firmware binaries into the GCS"
 	@$(ECHO) "     clean_package        - Clean, build and package the OpenPilot platform-dependent package"
-	@$(ECHO) "     package              - Build and package the OpenPilot platform-dependent package"
+	@$(ECHO) "     package              - Build and package the OpenPilot platform-dependent package (no clean)"
+	@$(ECHO) "     opfw_resource        - Generate resources to embed firmware binaries into the GCS"
 	@$(ECHO)
 	@$(ECHO) "   [Code Formatting]"
-	@$(ECHO) "     uncrustify_<source>  - Reformat <source> code. <source> can be flight or ground"
+	@$(ECHO) "     uncrustify_<source>  - Reformat <source> code according to the project's standards"
+	@$(ECHO) "                            Supported sources are ($(UNCRUSTIFY_TARGETS))"
 	@$(ECHO) "     uncrustify_all       - Reformat all source code"
 	@$(ECHO)
+	@$(ECHO) "   [Code Documentation]"
+	@$(ECHO) "     docs_<source>        - Generate HTML documentation for <source>"
+	@$(ECHO) "                            Supported sources are ($(DOCS_TARGETS))"
+	@$(ECHO) "     docs_all             - Generate HTML documentation for all"
+	@$(ECHO) "     docs_<source>_clean  - Delete generated documentation for <source>"
+	@$(ECHO) "     docs_all_clean       - Delete all generated documentation"
+	@$(ECHO)
 	@$(ECHO) "   Hint: Add V=1 to your command line to see verbose build output."
 	@$(ECHO)
-	@$(ECHO) "   Notes: All tool distribution files will be downloaded into $(DL_DIR)"
+	@$(ECHO) "  Notes: All tool distribution files will be downloaded into $(DL_DIR)"
-	@$(ECHO) "          All tools will be installed into $(TOOLS_DIR)"
+	@$(ECHO) "         All tools will be installed into $(TOOLS_DIR)"
-	@$(ECHO) "          All build output will be placed in $(BUILD_DIR)"
+	@$(ECHO) "         All build output will be placed in $(BUILD_DIR)"
+	@$(ECHO)
+	@$(ECHO) "  Tool download and install directories can be changed using environment variables:"
+	@$(ECHO) "         OPENPILOT_DL_DIR        full path to downloads directory [downloads if not set]"
+	@$(ECHO) "         OPENPILOT_TOOLS_DIR     full path to installed tools directory [tools if not set]"
+	@$(ECHO) "  More info: http://wiki.openpilot.org/display/Doc/OpenPilot+Build+System+Overview"
 	@$(ECHO)

androidgcs/res/layout/mycustommapview.xml

@@ -0,0 +1,38 @@
+<?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"
+   > 
+ <LinearLayout
+   android:orientation="vertical"
+   android:layout_width="fill_parent"
+   android:layout_height="wrap_content"
+   >
+    <TextView
+     android:id="@+id/longitude"
+     android:layout_width="fill_parent"
+     android:layout_height="fill_parent"
+     android:text="@string/longitude"
+  />
+  <TextView
+   android:id="@+id/latitude"
+   android:layout_width="fill_parent"
+     android:layout_height="fill_parent"
+     android:text="@string/latitude"
+  />
+  <TextView
+   android:id="@+id/altitude"
+   android:layout_width="fill_parent"
+     android:layout_height="fill_parent"
+     android:text="@string/altitude"
+  />
+ </LinearLayout>
+ <org.openpilot.androidgcs.MyCustomMapView
+  android:id="@+id/mapview"
+  android:layout_width="fill_parent"
+    android:layout_height="fill_parent"
+    android:clickable="true"
+  android:apiKey="098Goj3psJ2Vg_lGi2v0pRWf4mqxjBvh2FI4frg"
+ />
+</LinearLayout>

androidgcs/res/menu/map_menu.xml

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+<menu xmlns:android="http://schemas.android.com/apk/res/android" >
+    <item android:id="@+id/poi1" android:title="Set POI"></item>
+    <item android:id="@+id/poi2" android:title="Set FollowMe POI"></item>
+    <item android:id="@+id/view1" android:title="View Satellite images"></item>
+    <item android:id="@+id/view2" android:title="View Map"></item>
+    
+
+</menu>

androidgcs/res/values/strings.xml

@@ -32,6 +32,9 @@
 	<string name="alarms">Alarms</string>
 	<string name="txrate">TxRate: </string>
 	<string name="rxrate">RxRate: </string>
+	<string name="latitude">Latitude:</string>
+	<string name="longitude">Longitude:</string>
+	<string name="altitude">Altitude:</string>
 	<string name="tester">Tester</string>
 	<string name="_3dview">3DView</string>
 	<string name="tuning">Tuning</string>

androidgcs/src/org/openpilot/androidgcs/Controller.java

@@ -66,12 +66,14 @@ public class Controller extends ObjectManagerActivity {
 	private boolean leftJoystickHeld, rightJoystickHeld;
 
 	Timer sendTimer = new Timer();
-
+	TextView manualView;
+	
 	/** Called when the activity is first created. */
 	@Override
 	public void onCreate(Bundle savedInstanceState) {
 		super.onCreate(savedInstanceState);
 		setContentView(R.layout.controller);
+		manualView = (TextView) findViewById(R.id.manualControlValues);
 	}
 
 	Observer settingsUpdated = new Observer() {

androidgcs/src/org/openpilot/androidgcs/MyCustomMapView.java

@@ -0,0 +1,121 @@
+package org.openpilot.androidgcs;
+
+import java.util.Timer;
+import java.util.TimerTask;
+
+import com.google.android.maps.GeoPoint;
+import com.google.android.maps.MapView;
+
+import android.content.Context;
+import android.util.AttributeSet;
+import android.view.MotionEvent;
+
+public class MyCustomMapView extends MapView {
+    
+    // Define the interface we will interact with from our Map
+public interface OnLongpressListener {
+    public void onLongpress(MapView view, GeoPoint longpressLocation);
+}
+ 
+/**
+ * Time in ms before the OnLongpressListener is triggered.
+ */
+static final int LONGPRESS_THRESHOLD = 500;
+ 
+/**
+ * Keep a record of the center of the map, to know if the map
+ * has been panned.
+ */
+private GeoPoint lastMapCenter;
+ 
+private Timer longpressTimer = new Timer();
+private MyCustomMapView.OnLongpressListener longpressListener;
+
+ 
+public MyCustomMapView(Context context, String apiKey) {
+    super(context, apiKey);
+}
+
+public MyCustomMapView(Context context, AttributeSet attrs) {
+    super(context, attrs);
+}
+
+public MyCustomMapView(Context context, AttributeSet attrs, int defStyle) {
+    super(context, attrs, defStyle);
+}
+
+public void setOnLongpressListener(MyCustomMapView.OnLongpressListener listener) {
+    longpressListener = listener;
+}
+
+/**
+ * This method is called every time user touches the map,
+ * drags a finger on the map, or removes finger from the map.
+ */
+@Override
+public boolean onTouchEvent(MotionEvent event) {
+    handleLongpress(event);
+     
+    return super.onTouchEvent(event);
+}
+
+/**
+ * This method takes MotionEvents and decides whether or not
+ * a longpress has been detected. This is the meat of the
+ * OnLongpressListener.
+ *
+ * The Timer class executes a TimerTask after a given time,
+ * and we start the timer when a finger touches the screen.
+ *
+ * We then listen for map movements or the finger being
+ * removed from the screen. If any of these events occur
+ * before the TimerTask is executed, it gets cancelled. Else
+ * the listener is fired.
+ * 
+ * @param event
+ */
+private void handleLongpress(final MotionEvent event) {
+     
+    if (event.getAction() == MotionEvent.ACTION_DOWN) {
+        // Finger has touched screen.
+        longpressTimer = new Timer();
+        longpressTimer.schedule(new TimerTask() {
+            @Override
+            public void run() {
+                GeoPoint longpressLocation = getProjection().fromPixels((int)event.getX(),
+                        (int)event.getY());
+                 
+                /*
+                 * Fire the listener. We pass the map location
+                 * of the longpress as well, in case it is needed
+                 * by the caller.
+                 */
+                longpressListener.onLongpress(MyCustomMapView.this, longpressLocation);
+            }
+             
+        }, LONGPRESS_THRESHOLD);
+         
+        lastMapCenter = getMapCenter();
+    }
+     
+    if (event.getAction() == MotionEvent.ACTION_MOVE) {
+             
+        if (!getMapCenter().equals(lastMapCenter)) {
+            // User is panning the map, this is no longpress
+            longpressTimer.cancel();
+        }
+         
+        lastMapCenter = getMapCenter();
+    }
+     
+    if (event.getAction() == MotionEvent.ACTION_UP) {
+        // User has removed finger from map.
+        longpressTimer.cancel();
+    }
+
+        if (event.getPointerCount() > 1) {
+                    // This is a multitouch event, probably zooming.
+            longpressTimer.cancel();
+        }
+}
+}

androidgcs/src/org/openpilot/androidgcs/UAVLocation.java

@@ -49,13 +49,22 @@ import android.graphics.BitmapFactory;
 import android.graphics.Canvas;
 import android.graphics.Paint;
 import android.graphics.Point;
+import android.location.Location;
+import android.location.LocationListener;
+import android.location.LocationManager;
 import android.os.Bundle;
 import android.os.Handler;
 import android.os.IBinder;
 import android.util.Log;
+import android.view.ContextMenu;
+import android.view.ContextMenu.ContextMenuInfo;
 import android.view.Menu;
 import android.view.MenuInflater;
 import android.view.MenuItem;
+import android.view.KeyEvent;
+import android.view.View;
+import android.widget.TextView;
+import android.widget.Toast;
 
 import com.google.android.maps.GeoPoint;
 import com.google.android.maps.MapActivity;
@@ -72,9 +81,11 @@ public class UAVLocation extends MapActivity
 //	private static boolean WARN = LOGLEVEL > 1;
 	private static boolean DEBUG = LOGLEVEL > 0;
 
-	private MapView mapView;
+	//private MapView mapView;
+	private MyCustomMapView mapView;
 	private MapController mapController;
-
+	private GeoPoint mContextMenuGeoPoint = null;
+	
 	UAVObjectManager objMngr;
     boolean mBound = false;
     boolean mConnected = false;
@@ -84,23 +95,37 @@ public class UAVLocation extends MapActivity
     GeoPoint homeLocation;
     GeoPoint uavLocation;
 
+	boolean towerEnabled;
+	boolean gpsEnabled;
+	
+	LocationManager gpsLocationManager;
+	LocationManager towerLocationManager;
+	MyLocationListener gpsLocationListener;
+	MyLocationListener towerLocationListener;
+	private TextView myLongitude, myLatitude, myAltitude;
+    
     @Override public void onCreate(Bundle icicle) {
-		super.onCreate(icicle);
+    	super.onCreate(icicle);
-		setContentView(R.layout.map_layout);
+        setContentView(R.layout.mycustommapview);
-		mapView = (MapView)findViewById(R.id.map_view);
+        mapView = (MyCustomMapView)findViewById(R.id.mapview);
 		mapController = mapView.getController();
+	    registerForContextMenu(mapView);
 
 		mapView.displayZoomControls(true);
 		Double lat = 37.422006*1E6;
 		Double lng = -122.084095*1E6;
 		homeLocation = new GeoPoint(lat.intValue(), lng.intValue());
 		uavLocation = homeLocation;
-		mapController.setCenter(homeLocation);
+		//mapController.setCenter(homeLocation);
-		mapController.setZoom(18);
+		mapController.setZoom(16);
 
 		List<Overlay> overlays = mapView.getOverlays();
 		UAVOverlay myOverlay = new UAVOverlay();
 		overlays.add(myOverlay);
+		
+		  myLongitude = (TextView)findViewById(R.id.longitude);
+		  myLatitude = (TextView)findViewById(R.id.latitude);
+		  myAltitude = (TextView)findViewById(R.id.altitude);
 
 		MyLocationOverlay myLocationOverlay = new MyLocationOverlay(this, mapView);
 		myLocationOverlay.enableMyLocation();
@@ -108,15 +133,159 @@ public class UAVLocation extends MapActivity
 		overlays.add(myLocationOverlay);
 
 		mapView.postInvalidate();
+		gpsLocationListener = new MyLocationListener();
+		towerLocationListener = new MyLocationListener();
+		gpsLocationManager = (LocationManager) getSystemService(Context.LOCATION_SERVICE);
+		towerLocationManager = (LocationManager) getSystemService(Context.LOCATION_SERVICE);
 
+		CheckTowerAndGpsStatus();
+		if(gpsEnabled)
+		{
+			gpsLocationManager.requestLocationUpdates( LocationManager.GPS_PROVIDER, 0, 0, gpsLocationListener);
+	
+			  //Get the current location in start-up
+			Location last = gpsLocationManager.getLastKnownLocation(LocationManager.GPS_PROVIDER);
+			if(last != null)
+			{
+			  GeoPoint initGeoPoint = new GeoPoint(
+			   (int)(last.getLatitude()*1000000),
+			   (int)(last.getLongitude()*1000000));			  
+			   CenterLocation(initGeoPoint,(last.getAltitude()));
+			}
+		}
+		mapView.setOnLongpressListener(new MyCustomMapView.OnLongpressListener() {
+        public void onLongpress(final MapView view, final GeoPoint longpressLocation) {
+        	mContextMenuGeoPoint = longpressLocation;
+            runOnUiThread(new Runnable() {
+            public void run() {
+                // Insert your longpress action here
+            	//Toast.makeText(mapView.getContext(), "You pressed here: Lat:", Toast.LENGTH_LONG).show();
+            	openContextMenu(view);
+            }
+        });
+        }
+    });
+    }
+
+    private void CenterLocation(GeoPoint centerGeoPoint, double Altitude)
+    {
+    	mapController.animateTo(centerGeoPoint);
+     
+    
+	     myLongitude.setText("Longitude: "+
+	      String.valueOf((float)centerGeoPoint.getLongitudeE6()/1000000)
+	      );
+	     myLatitude.setText("Latitude: "+
+	    	      String.valueOf((float)centerGeoPoint.getLatitudeE6()/1000000)
+	    	      );
+	     myAltitude.setText("Altitude: "+
+	    	      String.valueOf(Altitude)
+	    	      );
+    };
+
+	private void CheckTowerAndGpsStatus() {
+		towerEnabled = towerLocationManager
+				.isProviderEnabled(LocationManager.NETWORK_PROVIDER);
+		gpsEnabled = gpsLocationManager
+				.isProviderEnabled(LocationManager.GPS_PROVIDER);
 	}
-
+    
-	//@Override
+    @Override
-	@Override
 	protected boolean isRouteDisplayed() {
 		// IMPORTANT: This method must return true if your Activity // is displaying driving directions. Otherwise return false.
 		return false;
 	}
+	
+	  @Override
+	  public void onCreateContextMenu(ContextMenu menu, View v,
+	      ContextMenuInfo menuInfo) {
+	    super.onCreateContextMenu(menu, v, menuInfo);
+	
+	    MenuInflater inflater = getMenuInflater();
+	    inflater.inflate(R.menu.map_menu, menu);
+	  }
+	  
+	  @Override
+	  public boolean onContextItemSelected(MenuItem item) {
+		  int lat = mContextMenuGeoPoint.getLatitudeE6();
+		  int lon = mContextMenuGeoPoint.getLongitudeE6();
+	      switch (item.getItemId()) {
+		      case R.id.poi1:
+
+		        return true;
+		      case R.id.poi2:
+		    	  Toast.makeText(mapView.getContext(), "You pressed here: Lat:" + lat/1000000.0 + " Lon:" + lon/1000000.0, Toast.LENGTH_LONG).show();
+		        return true;
+			case R.id.view1:
+				mapView.setSatellite(true);
+				mapView.invalidate();
+		        return true;
+			case R.id.view2:	
+				mapView.setSatellite(false);
+				mapView.invalidate();
+		        return true;
+			default:
+				return super.onContextItemSelected(item);
+	      }
+	  }
+
+	  
+	  /* Class My Location Listener */
+
+	  public class MyLocationListener implements LocationListener
+	  {
+		  @Override
+		  public void onLocationChanged(Location loc)
+		  {
+			GeoPoint myGeoPoint = new GeoPoint(
+				(int)(loc.getLatitude()*1000000),
+				(int)(loc.getLongitude()*1000000));
+			
+			CenterLocation(myGeoPoint,loc.getAltitude());
+		  }
+		  @Override
+		  public void onProviderDisabled(String provider)
+		  {
+			  Toast.makeText( getApplicationContext(),
+					  "Gps Disabled",
+					  Toast.LENGTH_SHORT ).show();
+		  }
+		  @Override
+		  public void onProviderEnabled(String provider)
+		  {
+			  Toast.makeText( getApplicationContext(),
+					  "Gps Enabled",
+					  Toast.LENGTH_SHORT).show();
+		  }
+
+		  @Override
+		  public void onStatusChanged(String provider, int status, Bundle extras)
+		  {
+		  }
+	  }/* End of Class MyLocationListener */
+	  
+	public boolean onKeyDown(int keyCode, KeyEvent event) {
+		switch (keyCode) {
+			case KeyEvent.KEYCODE_DPAD_UP:
+				mapController.zoomIn();
+				break;
+			case KeyEvent.KEYCODE_DPAD_DOWN:
+				mapController.zoomOut();
+				break;
+			case KeyEvent.KEYCODE_DPAD_LEFT:
+				mapController.setZoom(17);
+				mapView.setSatellite(true);
+				mapView.invalidate();
+				break;
+			case KeyEvent.KEYCODE_DPAD_RIGHT:	
+				mapController.setZoom(17);
+				mapView.setSatellite(false);
+				mapView.invalidate();	
+				break;
+				
+		}
+		return super.onKeyDown(keyCode, event);
+	}
 
 	public class UAVOverlay extends Overlay {
 		Bitmap homeSymbol = BitmapFactory.decodeResource(getResources(), R.drawable.ic_home);

flight/.gitattributes

@@ -15,3 +15,6 @@
 Makefile	text eol=lf
 Makefile.*	text eol=lf
 *.mk		text eol=lf
+
+*.h.template	text eol=lf
+*.c.template	text eol=lf

flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj

@@ -17,8 +17,8 @@
 		65904F2614632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = "<group>"; };
 		65904F2714632C1700FD9482 /* board-info.mk */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = "board-info.mk"; sourceTree = "<group>"; };
 		65904F2814632C1700FD9482 /* version-info.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = "version-info.py"; sourceTree = "<group>"; };
-		65904F2914632C1700FD9482 /* firmwareinfotemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmwareinfotemplate.c; sourceTree = "<group>"; };
+		65904F2914632C1700FD9482 /* firmware_info.c.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = firmware_info.c.template; sourceTree = "<group>"; };
-		65904F2A14632C1700FD9482 /* gcsversioninfotemplate.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcsversioninfotemplate.h; sourceTree = "<group>"; };
+		65904F2A14632C1700FD9482 /* gcs_version_info.h.template */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gcs_version_info.h.template; sourceTree = "<group>"; };
 		65904F2D14632C1700FD9482 /* README.txt */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = README.txt; sourceTree = "<group>"; };
 		65904F2E14632C1700FD9482 /* shell_script.reg */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = shell_script.reg; sourceTree = "<group>"; };
 		65904F2F14632C1700FD9482 /* install */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = install; sourceTree = "<group>"; };
@@ -79,8 +79,8 @@
 		65904F1A14632C1700FD9482 /* templates */ = {
 			isa = PBXGroup;
 			children = (
-				65904F2914632C1700FD9482 /* firmwareinfotemplate.c */,
+				65904F2914632C1700FD9482 /* firmware_info.c.template */,
-				65904F2A14632C1700FD9482 /* gcsversioninfotemplate.h */,
+				65904F2A14632C1700FD9482 /* gcs_version_info.h.template */,
 			);
 			path = templates;
 			sourceTree = "<group>";

flight/Project/gdb/osd

@@ -0,0 +1,4 @@
+define connect
+	target remote localhost:3333
+	file ./build/fw_osd/fw_osd.elf
+end

flight/libraries/WorldMagModel.c

@@ -834,7 +834,7 @@ int WMM_PcupHigh(float *Pcup, float *dPcup, float x, uint16_t nMax)
      * Note: OP code change to avoid floating point equality test.
      * Was: if (fabs(x) == 1.0)
      */
-	if (fabs(x) - 1.0f < 1e-9f)
+	if (fabsf(x) - 1.0f < 1e-9f)
 	{
 	    FREE(PreSqr);
 	    FREE(f2);

flight/libraries/inc/insgps.h

@@ -59,11 +59,13 @@ void INSCovariancePrediction(float dT);
 void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
 
 void INSResetP(float PDiag[13]);
+void INSGetP(float PDiag[13]);
 void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
-void INSSetPosVelVar(float PosVar, float VelVar);
+void INSSetPosVelVar(float PosVar[3], float VelVar[3]);
 void INSSetGyroBias(float gyro_bias[3]);
 void INSSetAccelVar(float accel_var[3]);
 void INSSetGyroVar(float gyro_var[3]);
+void INSSetGyroBiasVar(float gyro_bias_var[3]);
 void INSSetMagNorth(float B[3]);
 void INSSetMagVar(float scaled_mag_var[3]);
 void INSSetBaroVar(float baro_var);

flight/libraries/inc/packet_handler.h

@@ -1,16 +1,16 @@
 /**
- ******************************************************************************
+******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
+* @addtogroup OpenPilotSystem OpenPilot System
- * @{
+* @{
- * @addtogroup OpenPilotLibraries OpenPilot System Libraries
+* @addtogroup OpenPilotLibraries OpenPilot System Libraries
- * @{
+* @{
- *
+*
- * @file       packet_handler.h
+* @file       packet_handler.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+* @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
- * @brief      A packet handler for handeling radio packet transmission.
+* @brief      A packet handler for handeling radio packet transmission.
- * @see        The GNU Public License (GPL) Version 3
+* @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
@@ -41,66 +41,66 @@
 
 // Public types
 typedef enum {
-	PACKET_TYPE_NONE = 0,
+    PACKET_TYPE_NONE = 0,
-	PACKET_TYPE_STATUS,              // broadcasts status of this modem
+    PACKET_TYPE_STATUS,              // broadcasts status of this modem
-	PACKET_TYPE_CON_REQUEST,         // request a connection to another modem
+    PACKET_TYPE_CON_REQUEST,         // request a connection to another modem
-	PACKET_TYPE_DATA,                // data packet (packet contains user data)
+    PACKET_TYPE_DATA,                // data packet (packet contains user data)
-	PACKET_TYPE_DUPLICATE_DATA,      // a duplicate data packet
+    PACKET_TYPE_DUPLICATE_DATA,      // a duplicate data packet
-	PACKET_TYPE_PPM,                 // PPM relay values
+    PACKET_TYPE_PPM,                 // PPM relay values
-	PACKET_TYPE_ACK,                 // Acknowlege the receipt of a packet
+    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
-	PACKET_TYPE_NACK,                // Acknowlege the receipt of an uncorrectable packet
 } PHPacketType;
 
 typedef struct {
-	uint32_t destination_id;
+    uint32_t destination_id;
-	uint16_t seq_num;
+    portTickType prev_tx_time;
-	uint8_t type;
+    uint16_t seq_num;
-	uint8_t data_size;
+    uint8_t type;
+    uint8_t data_size;
 } 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)
 typedef struct {
-	PHPacketHeader header;
+    PHPacketHeader header;
-	uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
+    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;
+    PHPacketHeader header;
-	portTickType packet_recv_time;
+    uint8_t ecc[RS_ECC_NPARITY];
-	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;
+    PHPacketHeader header;
-	uint16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS];
+    int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS];
-	uint8_t ecc[RS_ECC_NPARITY];
+    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;
+    PHPacketHeader header;
-        uint32_t source_id;
+    uint32_t source_id;
-	uint8_t link_quality;
+    uint8_t link_quality;
-	int8_t received_rssi;
+    int8_t received_rssi;
-	uint8_t ecc[RS_ECC_NPARITY];
+    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 {
-	PHPacketHeader header;
+    PHPacketHeader header;
-        uint32_t source_id;
+    uint32_t source_id;
-	uint32_t min_frequency;
+    uint32_t min_frequency;
-	uint32_t max_frequency;
+    uint32_t max_frequency;
-	uint32_t channel_spacing;
+    uint32_t channel_spacing;
-	OPLinkSettingsMainPortOptions main_port;
+    portTickType status_rx_time;
-	OPLinkSettingsFlexiPortOptions flexi_port;
+    OPLinkSettingsMainPortOptions main_port;
-	OPLinkSettingsVCPPortOptions vcp_port;
+    OPLinkSettingsFlexiPortOptions flexi_port;
-	OPLinkSettingsComSpeedOptions com_speed;
+    OPLinkSettingsVCPPortOptions vcp_port;
-	uint8_t ecc[RS_ECC_NPARITY];
+    OPLinkSettingsComSpeedOptions com_speed;
+    uint8_t ecc[RS_ECC_NPARITY];
 } PHConnectionPacket, *PHConnectionPacketHandle;
 
 #endif // __PACKET_HANDLER_H__

flight/libraries/insgps13state.c

@@ -141,7 +141,19 @@ void INSResetP(float PDiag[NUMX])
 	}
 }
 
-void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
+void INSGetP(float PDiag[NUMX])
+{
+	uint8_t i;
+
+	// retrieve diagonal elements (aka state variance)
+	for (i=0;i<NUMX;i++){
+		if (PDiag != 0){
+			PDiag[i] = P[i][i];
+		}
+	}
+}
+
+void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3])
 {
 	/* Note: accel_bias not used in 13 state INS */
 	X[0] = pos[0];
@@ -179,14 +191,14 @@ void INSPosVelReset(float pos[3], float vel[3])
 	X[5] = vel[2];	
 }
 
-void INSSetPosVelVar(float PosVar, float VelVar)
+void INSSetPosVelVar(float PosVar[3], float VelVar[3])
 {
-	R[0] = PosVar;
+	R[0] = PosVar[0];
-	R[1] = PosVar;
+	R[1] = PosVar[1];
-	R[2] = PosVar;
+	R[2] = PosVar[2];
-	R[3] = VelVar;
+	R[3] = VelVar[0];
-	R[4] = VelVar;
+	R[4] = VelVar[1];
-	R[5] = VelVar;
+	R[5] = VelVar[2];
 }
 
 void INSSetGyroBias(float gyro_bias[3])
@@ -210,6 +222,13 @@ void INSSetGyroVar(float gyro_var[3])
 	Q[2] = gyro_var[2];
 }
 
+void INSSetGyroBiasVar(float gyro_bias_var[3])
+{
+	Q[6] = gyro_bias_var[0];
+	Q[7] = gyro_bias_var[1];
+	Q[8] = gyro_bias_var[2];
+}
+
 void INSSetMagVar(float scaled_mag_var[3])
 {
 	R[6] = scaled_mag_var[0];

flight/libraries/op_dfu.c

@@ -78,7 +78,7 @@ extern uint8_t JumpToApp;
 void sendData(uint8_t * buf, uint16_t size);
 uint32_t CalcFirmCRC(void);
 
-void DataDownload(DownloadAction action) {
+void DataDownload(__attribute__((unused)) DownloadAction action) {
 	if ((DeviceState == downloading)) {
 
 		uint8_t packetSize;

flight/libraries/paths.c

@@ -24,7 +24,9 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#include "pios.h"
+#include <pios.h>
+#include <pios_math.h>
+
 #include "paths.h"
 
 #include "uavobjectmanager.h" // <--.
@@ -32,8 +34,8 @@
 // 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_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_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);
 
 /**
@@ -46,26 +48,26 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
  */
 void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
 {
-	switch(mode) {
+    switch (mode) {
-		case PATHDESIRED_MODE_FLYVECTOR:
+        case PATHDESIRED_MODE_FLYVECTOR:
-		case PATHDESIRED_MODE_DRIVEVECTOR:
+        case PATHDESIRED_MODE_DRIVEVECTOR:
-			return path_vector(start_point, end_point, cur_point, status);
+            return path_vector(start_point, end_point, cur_point, status);
-			break;
+            break;
-		case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+        case PATHDESIRED_MODE_FLYCIRCLERIGHT:
-		case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+        case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
-			return path_circle(start_point, end_point, cur_point, status, 1);
+            return path_circle(start_point, end_point, cur_point, status, 1);
-			break;
+            break;
-		case PATHDESIRED_MODE_FLYCIRCLELEFT:
+        case PATHDESIRED_MODE_FLYCIRCLELEFT:
-		case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+        case PATHDESIRED_MODE_DRIVECIRCLELEFT:
-			return path_circle(start_point, end_point, cur_point, status, 0);
+            return path_circle(start_point, end_point, cur_point, status, 0);
-			break;
+            break;
-		case PATHDESIRED_MODE_FLYENDPOINT:
+        case PATHDESIRED_MODE_FLYENDPOINT:
-		case PATHDESIRED_MODE_DRIVEENDPOINT:
+        case PATHDESIRED_MODE_DRIVEENDPOINT:
-		default:
+        default:
-			// use the endpoint as default failsafe if called in unknown modes
+            // use the endpoint as default failsafe if called in unknown modes
-			return path_endpoint(start_point, end_point, cur_point, status);
+            return path_endpoint(start_point, end_point, cur_point, status);
-			break;
+            break;
-	}
+    }
 }
 
 /**
@@ -75,38 +77,38 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
  * @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)
+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 path_north, path_east, diff_north, diff_east;
-	float dist_path, dist_diff;
+    float dist_path, dist_diff;
 
-	// we do not correct in this mode
+    // we do not correct in this mode
-	status->correction_direction[0] = status->correction_direction[1] = 0;
+    status->correction_direction[0] = status->correction_direction[1] = 0;
 
-	// Distance to go
+    // Distance to go
-	path_north = end_point[0] - start_point[0];
+    path_north = end_point[0] - start_point[0];
-	path_east = end_point[1] - start_point[1];
+    path_east = end_point[1] - start_point[1];
 
-	// Current progress location relative to end
+    // Current progress location relative to end
-	diff_north = end_point[0] - cur_point[0];
+    diff_north = end_point[0] - cur_point[0];
-	diff_east = end_point[1] - cur_point[1];
+    diff_east = end_point[1] - cur_point[1];
 
-	dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
+    dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east);
-	dist_path = sqrtf( path_north * path_north + path_east * path_east );
+    dist_path = sqrtf(path_north * path_north + path_east * path_east);
 
 	if (dist_diff < 1e-6f ) {
-		status->fractional_progress = 1;
+        status->fractional_progress = 1;
-		status->error = 0;
+        status->error = 0;
-		status->path_direction[0] = status->path_direction[1] = 0;
+        status->path_direction[0] = status->path_direction[1] = 0;
-		return;
+        return;
-	}
+    }
 
-	status->fractional_progress = 1 - dist_diff / (1 + dist_path);
+    status->fractional_progress = 1 - dist_diff / (1 + dist_path);
-	status->error = dist_diff;
+    status->error = dist_diff;
 
-	// Compute direction to travel
+    // Compute direction to travel
-	status->path_direction[0] = diff_north / dist_diff;
+    status->path_direction[0] = diff_north / dist_diff;
-	status->path_direction[1] = diff_east / dist_diff;
+    status->path_direction[1] = diff_east / dist_diff;
 
 }
 
@@ -117,50 +119,50 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
  * @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)
+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 path_north, path_east, diff_north, diff_east;
-	float dist_path;
+    float dist_path;
-	float dot;
+    float dot;
-	float normal[2];
+    float normal[2];
 
-	// Distance to go
+    // Distance to go
-	path_north = end_point[0] - start_point[0];
+    path_north = end_point[0] - start_point[0];
-	path_east = end_point[1] - start_point[1];
+    path_east = end_point[1] - start_point[1];
 
-	// Current progress location relative to start
+    // Current progress location relative to start
-	diff_north = cur_point[0] - start_point[0];
+    diff_north = cur_point[0] - start_point[0];
-	diff_east = cur_point[1] - start_point[1];
+    diff_east = cur_point[1] - start_point[1];
 
-	dot = path_north * diff_north + path_east * diff_east;
+    dot = path_north * diff_north + path_east * diff_east;
-	dist_path = sqrtf( path_north * path_north + path_east * path_east );
+    dist_path = sqrtf(path_north * path_north + path_east * path_east);
 
 	if (dist_path < 1e-6f){
-		// if the path is too short, we cannot determine vector direction.
+        // if the path is too short, we cannot determine vector direction.
-		// Fly towards the endpoint to prevent flying away,
+        // Fly towards the endpoint to prevent flying away,
-		// but assume progress=1 either way.
+        // but assume progress=1 either way.
-		path_endpoint( start_point, end_point, cur_point, status );
+        path_endpoint(start_point, end_point, cur_point, status);
-		status->fractional_progress = 1;
+        status->fractional_progress = 1;
-		return;
+        return;
-	}
+    }
 
-	// Compute the normal to the path
+    // Compute the normal to the path
-	normal[0] = -path_east / dist_path;
+    normal[0] = -path_east / dist_path;
-	normal[1] = path_north / dist_path;
+    normal[1] = path_north / dist_path;
 
-	status->fractional_progress = dot / (dist_path * dist_path);
+    status->fractional_progress = dot / (dist_path * dist_path);
-	status->error = normal[0] * diff_north + normal[1] * diff_east;
+    status->error = normal[0] * diff_north + normal[1] * diff_east;
 
-	// Compute direction to correct error
+    // Compute direction to correct error
-	status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
+    status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
-	status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
+    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
+    // Now just want magnitude of error
-	status->path_direction[0] = path_north / dist_path;
+    status->error = fabs(status->error);
-	status->path_direction[1] = path_east / dist_path;
+
+    // Compute direction to travel
+    status->path_direction[0] = path_north / dist_path;
+    status->path_direction[1] = path_east / dist_path;
 
 }
 
@@ -173,54 +175,79 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
  */
 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_north, radius_east, diff_north, diff_east;
-	float radius,cradius;
+    float radius, cradius;
-	float normal[2];
+    float normal[2];
+    float progress;
+    float a_diff, a_radius;
 
-	// Radius
+    // Radius
-	radius_north = end_point[0] - start_point[0];
+    radius_north = end_point[0] - start_point[0];
-	radius_east = end_point[1] - start_point[1];
+    radius_east = end_point[1] - start_point[1];
 
-	// Current location relative to center
+    // Current location relative to center
-	diff_north = cur_point[0] - end_point[0];
+    diff_north = cur_point[0] - end_point[0];
-	diff_east = cur_point[1] - end_point[1];
+    diff_east = cur_point[1] - end_point[1];
 
-	radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
+    radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2));
-	cradius = sqrtf(  diff_north * diff_north   +   diff_east * diff_east );
+    cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2));
 
 	if (cradius < 1e-6f) {
-		// cradius is zero, just fly somewhere and make sure correction is still a normal
+        // cradius is zero, just fly somewhere and make sure correction is still a normal
-		status->fractional_progress = 1;
+        status->fractional_progress = 1;
-		status->error = radius;
+        status->error = radius;
-		status->correction_direction[0] = 0;
+        status->correction_direction[0] = 0;
-		status->correction_direction[1] = 1;
+        status->correction_direction[1] = 1;
-		status->path_direction[0] = 1;
+        status->path_direction[0] = 1;
-		status->path_direction[1] = 0;
+        status->path_direction[1] = 0;
-		return;
+        return;
-	}
+    }
 
-	if (clockwise) {
+    if (clockwise) {
-		// Compute the normal to the radius clockwise
+        // Compute the normal to the radius clockwise
-		normal[0] = -diff_east / cradius;
+        normal[0] = -diff_east / cradius;
-		normal[1] = diff_north / cradius;
+        normal[1] = diff_north / cradius;
-	} else {
+    } else {
-		// Compute the normal to the radius counter clockwise
+        // Compute the normal to the radius counter clockwise
-		normal[0] = diff_east / cradius;
+        normal[0] = diff_east / cradius;
-		normal[1] = -diff_north / 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
+    // normalize progress to 0..1
-	status->error = radius - cradius;
+    a_diff = atan2f(diff_north, diff_east);
+    a_radius = atan2f(radius_north, radius_east);
 
-	// Compute direction to correct error
+    if (a_diff < 0) {
-	status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius;
+        a_diff += 2.0f * M_PI_F;
-	status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
+    }
+    if (a_radius < 0) {
+        a_radius += 2.0f * M_PI_F;
+    }
 
-	// Compute direction to travel
+    progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F);
-	status->path_direction[0] = normal[0];
-	status->path_direction[1] = normal[1];
 
-	status->error = fabs(status->error);
+    if (progress < 0) {
+        progress += 1.0f;
+    } else if (progress >= 1.0f) {
+        progress -= 1.0f;
+    }
+
+    if (clockwise) {
+        progress = 1 - progress;
+    }
+
+    status->fractional_progress = progress;
+
+    // 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/printf2.c

@@ -74,7 +74,7 @@ char **environ = __env;
 /*==============================================================================
  * Close a file.
  */
-int _close(int file)
+int _close(__attribute__((unused)) int file)
 {
     return -1;
 }
@@ -82,7 +82,7 @@ int _close(int file)
 /*==============================================================================
  * Transfer control to a new process.
  */
-int _execve(char *name, char **argv, char **env)
+int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **argv, __attribute__((unused)) char **env)
 {
     errno = ENOMEM;
     return -1;
@@ -91,7 +91,7 @@ int _execve(char *name, char **argv, char **env)
 /*==============================================================================
  * Exit a program without cleaning up files.
  */
-void _exit( int code )
+void _exit( __attribute__((unused)) int code )
 {
 	/* Should we force a system reset? */
 	while( 1 )
@@ -112,7 +112,7 @@ int _fork(void)
 /*==============================================================================
  * Status of an open file.
  */
-int _fstat(int file, struct stat *st)
+int _fstat(__attribute__((unused)) int file, struct stat *st)
 {
     st->st_mode = S_IFCHR;
     return 0;
@@ -129,7 +129,7 @@ int _getpid(void)
 /*==============================================================================
  * Query whether output stream is a terminal.
  */
-int _isatty(int file)
+int _isatty(__attribute__((unused)) int file)
 {
     return 1;
 }
@@ -137,7 +137,7 @@ int _isatty(int file)
 /*==============================================================================
  * Send a signal.
  */
-int _kill(int pid, int sig)
+int _kill(__attribute__((unused)) int pid, __attribute__((unused)) int sig)
 {
     errno = EINVAL;
     return -1;
@@ -146,7 +146,7 @@ int _kill(int pid, int sig)
 /*==============================================================================
  * Establish a new name for an existing file.
  */
-int _link(char *old, char *new)
+int _link(__attribute__((unused)) char *old, __attribute__((unused)) char *new)
 {
     errno = EMLINK;
     return -1;
@@ -155,7 +155,7 @@ int _link(char *old, char *new)
 /*==============================================================================
  * Set position in a file.
  */
-int _lseek(int file, int ptr, int dir)
+int _lseek(__attribute__((unused)) int file, __attribute__((unused)) int ptr, __attribute__((unused)) int dir)
 {
     return 0;
 }
@@ -163,7 +163,7 @@ int _lseek(int file, int ptr, int dir)
 /*==============================================================================
  * Open a file.
  */
-int _open(const char *name, int flags, int mode)
+int _open(__attribute__((unused)) const char *name, __attribute__((unused)) int flags, __attribute__((unused)) int mode)
 {
     return -1;
 }
@@ -171,7 +171,7 @@ int _open(const char *name, int flags, int mode)
 /*==============================================================================
  * Read from a file.
  */
-int _read(int file, char *ptr, int len)
+int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
 {
     return 0;
 }
@@ -182,7 +182,7 @@ int _read(int file, char *ptr, int len)
  * example to a serial port for debugging, you should make your minimal write
  * capable of doing this.
  */
-int _write_r( void * reent, int file, char * ptr, int len )
+int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int file, __attribute__((unused)) char * ptr, __attribute__((unused)) int len )
 {
     return 0;
 }
@@ -220,7 +220,7 @@ caddr_t _sbrk(int incr)
 /*==============================================================================
  * Status of a file (by name).
  */
-int _stat(char *file, struct stat *st)
+int _stat(__attribute__((unused)) char *file, struct stat *st)
 {
     st->st_mode = S_IFCHR;
     return 0;
@@ -229,7 +229,7 @@ int _stat(char *file, struct stat *st)
 /*==============================================================================
  * Timing information for current process.
  */
-int _times(struct tms *buf)
+int _times(__attribute__((unused)) struct tms *buf)
 {
     return -1;
 }
@@ -237,7 +237,7 @@ int _times(struct tms *buf)
 /*==============================================================================
  * Remove a file's directory entry.
  */
-int _unlink(char *name)
+int _unlink(__attribute__((unused)) char *name)
 {
     errno = ENOENT;
     return -1;
@@ -246,7 +246,7 @@ int _unlink(char *name)
 /*==============================================================================
  * Wait for a child process.
  */
-int _wait(int *status)
+int _wait(__attribute__((unused)) int *status)
 {
     errno = ECHILD;
     return -1;

flight/libraries/sanitycheck.c

@@ -63,7 +63,7 @@ int32_t configuration_check()
     bool multirotor = true;
     uint8_t airframe_type;
     SystemSettingsAirframeTypeGet(&airframe_type);
-    switch(airframe_type) {
+    switch (airframe_type) {
         case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
         case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
         case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
@@ -73,6 +73,7 @@ int32_t configuration_check()
         case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
         case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
         case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
+        case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
             multirotor = true;
             break;
         default:
@@ -86,8 +87,8 @@ int32_t configuration_check()
     ManualControlSettingsFlightModeNumberGet(&num_modes);
     ManualControlSettingsFlightModePositionGet(modes);
 
-    for(uint32_t i = 0; i < num_modes; i++) {
+    for (uint32_t i = 0; i < num_modes; i++) {
-        switch(modes[i]) {
+        switch (modes[i]) {
             case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
                 if (multirotor) {
                     severity = SYSTEMALARMS_ALARM_ERROR;
@@ -122,9 +123,42 @@ int32_t configuration_check()
                 }
                 break;
             case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
-                if (coptercontrol){
+                if (coptercontrol) {
                     severity = SYSTEMALARMS_ALARM_ERROR;
-                } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
+                } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports Position Hold
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports AutoLand Mode
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports POI Mode
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports PathPlanner
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports ReturnToBase
                     severity = SYSTEMALARMS_ALARM_ERROR;
                 }
                 break;
@@ -141,7 +175,7 @@ int32_t configuration_check()
     }
 
     // TODO: Check on a multirotor no axis supports "None"
-    if(severity != SYSTEMALARMS_ALARM_OK)
+    if (severity != SYSTEMALARMS_ALARM_OK)
         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
     else
         AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
@@ -159,13 +193,13 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
 {
     // Make sure the modes have identical sizes
     if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
-            MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM)
+        MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM)
         return SYSTEMALARMS_ALARM_ERROR;
 
     uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
 
     // Get the different axis modes for this switch position
-    switch(index) {
+    switch (index) {
         case 1:
             ManualControlSettingsStabilization1SettingsGet(modes);
             break;
@@ -181,7 +215,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
 
     // For multirotors verify that nothing is set to "none"
     if (multirotor) {
-        for(uint32_t i = 0; i < NELEMENTS(modes); i++) {
+        for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
             if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE)
                 return SYSTEMALARMS_ALARM_ERROR;
         }

flight/modules/Actuator/actuator.c

@@ -152,12 +152,13 @@ MODULE_INITCALL(ActuatorInitialize, ActuatorStart)
  *
  * @return -1 if error, 0 if success
  */
-static void actuatorTask(void* parameters)
+static void actuatorTask(__attribute__((unused)) void* parameters)
 {
 	UAVObjEvent ev;
 	portTickType lastSysTime;
 	portTickType thisSysTime;
-	float dT = 0.0f;
+	float dTSeconds;
+	uint32_t dTMilliseconds;
 
 	ActuatorCommandData command;
 	ActuatorDesiredData desired;
@@ -208,13 +209,9 @@ static void actuatorTask(void* parameters)
 
 		// Check how long since last update
 		thisSysTime = xTaskGetTickCount();
-		// reuse dt in case of wraparound
+		dTMilliseconds = (thisSysTime == lastSysTime)? 1: (thisSysTime - lastSysTime) * portTICK_RATE_MS;
-		// 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;
+		dTSeconds = dTMilliseconds * 0.001f;
 
 		FlightStatusGet(&flightStatus);
 		ActuatorDesiredGet(&desired);
@@ -298,7 +295,7 @@ static void actuatorTask(void* parameters)
 			}
 
 			if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO))
-				status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
+				status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
 			else
 				status[ct] = -1;
 
@@ -372,7 +369,7 @@ static void actuatorTask(void* parameters)
 							   actuatorSettings.ChannelNeutral[i]);
 
 		// Store update time
-		command.UpdateTime = dT * 1000.0f;
+		command.UpdateTime = dTMilliseconds;
 		if (command.UpdateTime > command.MaxUpdateTime)
 			command.MaxUpdateTime = command.UpdateTime;
 		
@@ -723,12 +720,12 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData * actuato
 	}
 }
 
-static void ActuatorSettingsUpdatedCb(UAVObjEvent * ev)
+static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	actuator_settings_updated = true;
 }
 
-static void MixerSettingsUpdatedCb(UAVObjEvent * ev)
+static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	mixer_settings_updated = true;
 }

flight/modules/Airspeed/revolution/airspeed.c

@@ -131,7 +131,7 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart)
 /**
  * Module thread, should not return.
  */
-static void airspeedTask(void *parameters)
+static void airspeedTask(__attribute__((unused)) void *parameters)
 {
 	AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
 	
@@ -179,7 +179,7 @@ static void airspeedTask(void *parameters)
 	
 
 
-static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
+static void AirspeedSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	AirspeedSettingsGet(&airspeedSettings);
 	

flight/modules/Airspeed/revolution/baro_airspeed_etasv3.c

@@ -61,7 +61,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
 
 	//Check to see if airspeed sensor is returning airspeedSensor
 	airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed();
-	if (airspeedSensor->SensorValue==-1) {
+	if (airspeedSensor->SensorValue==(uint16_t)-1) {
 		airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
 		airspeedSensor->CalibratedAirspeed = 0;
 		return;

flight/modules/Altitude/altitude.c

@@ -40,7 +40,9 @@
 
 #include "hwsettings.h"
 #include "altitude.h"
+#if defined(PIOS_INCLUDE_BMP085)
 #include "baroaltitude.h"	// object that will be updated by the module
+#endif
 #if defined(PIOS_INCLUDE_HCSR04)
 #include "sonaraltitude.h"	// object that will be updated by the module
 #endif
@@ -57,12 +59,15 @@
 static xTaskHandle taskHandle;
 
 // down sampling variables
+#if defined(PIOS_INCLUDE_BMP085)
 #define alt_ds_size    4
 static int32_t alt_ds_temp = 0;
 static int32_t alt_ds_pres = 0;
 static int alt_ds_count = 0;
+#endif
 
 static bool altitudeEnabled;
+static uint8_t hwsettings_rcvrport;;
 
         // Private functions
 static void altitudeTask(void *parameters);
@@ -75,9 +80,11 @@ int32_t AltitudeStart()
 {
 
 	if (altitudeEnabled) {
+#if defined(PIOS_INCLUDE_BMP085)
 		BaroAltitudeInitialize();
+#endif
 #if defined(PIOS_INCLUDE_HCSR04)
-		SonarAltitudeInitialze();
+		SonarAltitudeInitialize();
 #endif
 
 		// Start main task
@@ -107,59 +114,65 @@ int32_t AltitudeInitialize()
 	}
 #endif
 
+#if defined(PIOS_INCLUDE_BMP085)
 	// init down-sampling data
 	alt_ds_temp = 0;
 	alt_ds_pres = 0;
 	alt_ds_count = 0;
-
+#endif
+	HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport);
 	return 0;
 }
 MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
 /**
  * Module thread, should not return.
  */
-static void altitudeTask(void *parameters)
+static void altitudeTask(__attribute__((unused)) void *parameters)
 {
-	BaroAltitudeData data;
 	portTickType lastSysTime;
 	
 #if defined(PIOS_INCLUDE_HCSR04)
 	SonarAltitudeData sonardata;
-	int32_t value=0,timeout=5;
+	int32_t value = 0, timeout = 5;
-	float coeff=0.25,height_out=0,height_in=0;
+	float coeff = 0.25, height_out = 0, height_in = 0;
-	PIOS_HCSR04_Init();
+	if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
-	PIOS_HCSR04_Trigger();
+		PIOS_HCSR04_Trigger();
+	}
 #endif
+#if defined(PIOS_INCLUDE_BMP085)
+	BaroAltitudeData data;
 	PIOS_BMP085_Init();
+#endif
 	
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	while (1)
 	{
 #if defined(PIOS_INCLUDE_HCSR04)
-		// Compute the current altitude (all pressures in kPa)
+		// Compute the current altitude
-		if(PIOS_HCSR04_Completed())
+		if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
-		{
+			if(PIOS_HCSR04_Completed()) {
-			value = PIOS_HCSR04_Get();
+				value = PIOS_HCSR04_Get();
-			if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
+				//from 3.4cm to 5.1m
-			{
+				if((value > 100) && (value < 15000)) {
-				height_in = value*0.00034;
+					height_in = value * 0.00034f / 2.0f;
-				height_out = (height_out * (1 - coeff)) + (height_in * coeff);
+					height_out = (height_out * (1 - coeff)) + (height_in * coeff);
-				sonardata.Altitude = height_out; // m/us
+					sonardata.Altitude = height_out; // m/us
+				}
+
+				// Update the AltitudeActual UAVObject
+				SonarAltitudeSet(&sonardata);
+				timeout = 5;
+				PIOS_HCSR04_Trigger();
+			}
+			if(!(timeout--)) {
+				//retrigger
+				timeout = 5;
+				PIOS_HCSR04_Trigger();
 			}
-			
-			// Update the AltitudeActual UAVObject
-			SonarAltitudeSet(&sonardata);
-			timeout=5;
-			PIOS_HCSR04_Trigger();
-		}
-		if(timeout--)
-		{
-			//retrigger
-			timeout=5;
-			PIOS_HCSR04_Trigger();
 		}
 #endif
+#if defined(PIOS_INCLUDE_BMP085)
 		// Update the temperature data
 		PIOS_BMP085_StartADC(TemperatureConv);
 #ifdef PIOS_BMP085_HAS_GPIOS
@@ -198,6 +211,7 @@ static void altitudeTask(void *parameters)
 			// Update the AltitudeActual UAVObject
 			BaroAltitudeSet(&data);
 		}
+#endif
 
 		// Delay until it is time to read the next sample
 		vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS);

flight/modules/Altitude/revolution/altitude.c

@@ -63,10 +63,6 @@ static void altitudeTask(void *parameters);
  */
 int32_t AltitudeStart()
 {	
-#if defined(PIOS_INCLUDE_HCSR04)
-	SonarAltitudeInitialze();
-#endif
-	
 	// Start main task
 	xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
 	PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);
@@ -81,22 +77,23 @@ int32_t AltitudeStart()
 int32_t AltitudeInitialize()
 {
 	BaroAltitudeInitialize();
-
+#if defined(PIOS_INCLUDE_HCSR04)
+	SonarAltitudeInitialize();
+#endif
 	return 0;
 }
 MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
 /**
  * Module thread, should not return.
  */
-static void altitudeTask(void *parameters)
+static void altitudeTask(__attribute__((unused)) void *parameters)
 {
 	BaroAltitudeData data;
 	
 #if defined(PIOS_INCLUDE_HCSR04)
 	SonarAltitudeData sonardata;
-	int32_t value=0,timeout=5;
+	int32_t value = 0, timeout = 10, sample_rate = 0;
-	float coeff=0.25,height_out=0,height_in=0;
+	float coeff = 0.25, height_out = 0, height_in = 0;
-	PIOS_HCSR04_Init();
 	PIOS_HCSR04_Trigger();
 #endif
 
@@ -113,27 +110,29 @@ static void altitudeTask(void *parameters)
 	while (1)
 	{
 #if defined(PIOS_INCLUDE_HCSR04)
-		// Compute the current altitude (all pressures in kPa)
+		// Compute the current altitude
-		if(PIOS_HCSR04_Completed())
+		// depends on baro samplerate
-		{
+		if(!(sample_rate--)) {
-			value = PIOS_HCSR04_Get();
+			if(PIOS_HCSR04_Completed()) {
-			if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
+				value = PIOS_HCSR04_Get();
-			{
+				//from 3.4cm to 5.1m
-				height_in = value*0.00034;
+				if((value > 100) && (value < 15000)) {
-				height_out = (height_out * (1 - coeff)) + (height_in * coeff);
+					height_in = value * 0.00034f / 2.0f;
-				sonardata.Altitude = height_out; // m/us
+					height_out = (height_out * (1 - coeff)) + (height_in * coeff);
+					sonardata.Altitude = height_out; // m/us
+				}
+
+				// Update the SonarAltitude UAVObject
+				SonarAltitudeSet(&sonardata);
+				timeout = 10;
+				PIOS_HCSR04_Trigger();
 			}
-			
+			if(!(timeout--)) {
-			// Update the AltitudeActual UAVObject
+				//retrigger
-			SonarAltitudeSet(&sonardata);
+				timeout = 10;
-			timeout=5;
+				PIOS_HCSR04_Trigger();
-			PIOS_HCSR04_Trigger();
+			}
-		}
+			sample_rate = 25;
-		if(timeout--)
-		{
-			//retrigger
-			timeout=5;
-			PIOS_HCSR04_Trigger();
 		}
 #endif
 		float temp, press;

flight/modules/AltitudeHold/altitudehold.c

@@ -119,7 +119,7 @@ float starting_altitude;
 /**
  * Module thread, should not return.
  */
-static void altitudeHoldTask(void *parameters)
+static void altitudeHoldTask(__attribute__((unused)) void *parameters)
 {
 	AltitudeHoldDesiredData altitudeHoldDesired;
 	StabilizationDesiredData stabilizationDesired;
@@ -387,7 +387,7 @@ static void altitudeHoldTask(void *parameters)
 	}
 }
 
-static void SettingsUpdatedCb(UAVObjEvent * ev)
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	AltitudeHoldSettingsGet(&altitudeHoldSettings);
 }

flight/modules/Attitude/attitude.c

@@ -95,6 +95,7 @@ static bool accel_filter_enabled = false;
 static float accels_filtered[3];
 static float grot_filtered[3];
 static float yawBiasRate = 0;
+static float rollPitchBiasRate = 0.0f;
 static float gyroGain = 0.42f;
 static int16_t accelbias[3];
 static float q[4] = {1,0,0,0};
@@ -176,7 +177,7 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
  
 int32_t accel_test;
 int32_t gyro_test;
-static void AttitudeTask(void *parameters)
+static void AttitudeTask(__attribute__((unused)) void *parameters)
 {
 	uint8_t init = 0;
 	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
@@ -219,17 +220,19 @@ static void AttitudeTask(void *parameters)
 		FlightStatusGet(&flightStatus);
 		
 		if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
-			// For first 7 seconds use accels to get gyro bias
+			// Use accels to initialise attitude and calculate gyro bias
 			accelKp = 1.0f;
-			accelKi = 0.9f;
+			accelKi = 0.0f;
 			yawBiasRate = 0.01f;
+			rollPitchBiasRate = 0.01f;
 			accel_filter_enabled = false;
 			init = 0;
 		}
 		else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
 			accelKp = 1.0f;
-			accelKi = 0.9f;
+			accelKi = 0.0f;
 			yawBiasRate = 0.01f;
+			rollPitchBiasRate = 0.01f;
 			accel_filter_enabled = false;
 			init = 0;
 		} else if (init == 0) {
@@ -237,6 +240,7 @@ static void AttitudeTask(void *parameters)
 			AttitudeSettingsAccelKiGet(&accelKi);
 			AttitudeSettingsAccelKpGet(&accelKp);
 			AttitudeSettingsYawBiasRateGet(&yawBiasRate);
+			rollPitchBiasRate = 0.0f;
 			if (accel_alpha > 0.0f)
 				accel_filter_enabled = true;
 			init = 1;
@@ -359,7 +363,11 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
 		gyros->y += gyro_correct_int[1];
 		gyros->z += gyro_correct_int[2];
 	}
-	
+
+	// Force the roll & pitch gyro rates to average to zero during initialisation
+	gyro_correct_int[0] += - gyros->x * rollPitchBiasRate;
+	gyro_correct_int[1] += - gyros->y * rollPitchBiasRate;
+		
 	// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
 	// and make it average zero (weakly)
 	gyro_correct_int[2] += - gyros->z * yawBiasRate;
@@ -431,6 +439,10 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
 		gyrosData->z += gyro_correct_int[2];
 	}
 
+	// Force the roll & pitch gyro rates to average to zero during initialisation
+	gyro_correct_int[0] += - gyrosData->x * rollPitchBiasRate;
+	gyro_correct_int[1] += - gyrosData->y * rollPitchBiasRate;
+
 	// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
 	// and make it average zero (weakly)
 	gyro_correct_int[2] += - gyrosData->z * yawBiasRate;
@@ -460,7 +472,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
 	portTickType thisSysTime = xTaskGetTickCount();
 	static portTickType lastSysTime = 0;
 	
-	dT = (thisSysTime == lastSysTime) ? 0.001f : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
+	dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f;
 	lastSysTime = thisSysTime;
 	
 	// Bad practice to assume structure order, but saves memory
@@ -547,10 +559,10 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
 		q[2] = 0;
 		q[3] = 0;
 	} else {
-		q[0] = q[0]/qmag;
+		q[0] = q[0] / qmag;
-		q[1] = q[1]/qmag;
+		q[1] = q[1] / qmag;
-		q[2] = q[2]/qmag;
+		q[2] = q[2] / qmag;
-		q[3] = q[3]/qmag;
+		q[3] = q[3] / qmag;
 	}
 	
 	AttitudeActualData attitudeActual;
@@ -564,7 +576,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
 	AttitudeActualSet(&attitudeActual);
 }
 
-static void settingsUpdatedCb(UAVObjEvent * objEv) {
+static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) {
 	AttitudeSettingsData attitudeSettings;
 	AttitudeSettingsGet(&attitudeSettings);
 	

flight/modules/Attitude/revolution/attitude.c

@@ -65,6 +65,8 @@
 #include "homelocation.h"
 #include "magnetometer.h"
 #include "positionactual.h"
+#include "ekfconfiguration.h"
+#include "ekfstatevariance.h"
 #include "revocalibration.h"
 #include "revosettings.h"
 #include "velocityactual.h"
@@ -103,10 +105,15 @@ static xQueueHandle gpsVelQueue;
 static AttitudeSettingsData attitudeSettings;
 static HomeLocationData homeLocation;
 static RevoCalibrationData revoCalibration;
+static EKFConfigurationData ekfConfiguration;
 static RevoSettingsData revoSettings;
-static bool gyroBiasSettingsUpdated = false;
+static FlightStatusData flightStatus;
 const uint32_t SENSOR_QUEUE_SIZE = 10;
 
+static bool volatile variance_error = true;
+static bool volatile initialization_required = true;
+static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
+
 // Private functions
 static void AttitudeTask(void *parameters);
 
@@ -116,6 +123,25 @@ static void settingsUpdatedCb(UAVObjEvent * objEv);
 
 static int32_t getNED(GPSPositionData * gpsPosition, float * NED);
 
+// check for invalid values
+static inline bool invalid(float data) {
+	if ( isnan(data) || isinf(data) ){
+		return true;
+	}
+	return false;
+}
+
+// check for invalid variance values
+static inline bool invalid_var(float data) {
+	if ( invalid(data) ) {
+		return true;
+	}
+	if ( data < 1e-15f ) { // var should not be close to zero. And not negative either.
+		return true;
+	}
+	return false;
+}
+
 /**
  * API for sensor fusion algorithms:
  * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
@@ -139,6 +165,9 @@ int32_t AttitudeInitialize(void)
 	VelocityActualInitialize();
 	RevoSettingsInitialize();
 	RevoCalibrationInitialize();
+	EKFConfigurationInitialize();
+	EKFStateVarianceInitialize();
+	FlightStatusInitialize();
 	
 	// Initialize this here while we aren't setting the homelocation in GPS
 	HomeLocationInitialize();
@@ -146,24 +175,26 @@ int32_t AttitudeInitialize(void)
 	// Initialize quaternion
 	AttitudeActualData attitude;
 	AttitudeActualGet(&attitude);
-	attitude.q1 = 1;
+	attitude.q1 = 1.0f;
-	attitude.q2 = 0;
+	attitude.q2 = 0.0f;
-	attitude.q3 = 0;
+	attitude.q3 = 0.0f;
-	attitude.q4 = 0;
+	attitude.q4 = 0.0f;
 	AttitudeActualSet(&attitude);
 	
 	// Cannot trust the values to init right above if BL runs
 	GyrosBiasData gyrosBias;
 	GyrosBiasGet(&gyrosBias);
-	gyrosBias.x = 0;
+	gyrosBias.x = 0.0f;
-	gyrosBias.y = 0;
+	gyrosBias.y = 0.0f;
-	gyrosBias.z = 0;
+	gyrosBias.z = 0.0f;
 	GyrosBiasSet(&gyrosBias);
 	
 	AttitudeSettingsConnectCallback(&settingsUpdatedCb);
 	RevoSettingsConnectCallback(&settingsUpdatedCb);
 	RevoCalibrationConnectCallback(&settingsUpdatedCb);
 	HomeLocationConnectCallback(&settingsUpdatedCb);
+	EKFConfigurationConnectCallback(&settingsUpdatedCb);
+	FlightStatusConnectCallback(&settingsUpdatedCb);
 
 	return 0;
 }
@@ -204,10 +235,9 @@ MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
 /**
  * Module thread, should not return.
  */
-static void AttitudeTask(void *parameters)
+static void AttitudeTask(__attribute__((unused)) void *parameters)
 {
-	bool first_run = true;
+
-	uint32_t last_algorithm;
 	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 
 	// Force settings update to make sure rotation loaded
@@ -216,21 +246,19 @@ static void AttitudeTask(void *parameters)
 	// Wait for all the sensors be to read
 	vTaskDelay(100);
 
-	// Invalidate previous algorithm to trigger a first run
+	// Main task loop - TODO: make it run as delayed callback
-	last_algorithm = 0xfffffff;
-
-	// Main task loop
 	while (1) {
 
 		int32_t ret_val = -1;
 
-		if (last_algorithm != revoSettings.FusionAlgorithm) {
+		bool first_run = false;
-			last_algorithm = revoSettings.FusionAlgorithm;
+		if (initialization_required) {
+			initialization_required = false;
 			first_run = true;
 		}
 
 		// This  function blocks on data queue
-		switch (revoSettings.FusionAlgorithm ) {
+		switch (running_algorithm ) {
 			case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
 				ret_val = updateAttitudeComplementary(first_run);
 				break;
@@ -245,8 +273,9 @@ static void AttitudeTask(void *parameters)
 				break;
 		}
 
-		if(ret_val == 0)
+		if(ret_val != 0) {
-			first_run = false;
+			initialization_required = true;
+		}
 
 		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
 	}
@@ -283,8 +312,6 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	AccelsGet(&accelsData);
 
 	// During initialization and 
-	FlightStatusData flightStatus;
-	FlightStatusGet(&flightStatus);
 	if(first_run) {
 #if defined(PIOS_INCLUDE_HMC5883)
 		// To initialize we need a valid mag reading
@@ -294,16 +321,36 @@ static int32_t updateAttitudeComplementary(bool first_run)
 		MagnetometerGet(&magData);
 #else
 		MagnetometerData magData;
-		magData.x = 100;
+		magData.x = 100.0f;
-		magData.y = 0;
+		magData.y = 0.0f;
-		magData.z = 0;
+		magData.z = 0.0f;
 #endif
 		AttitudeActualData attitudeActual;
 		AttitudeActualGet(&attitudeActual);
 		init = 0;
-		attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
+
-		attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
+		// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
-		attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
+		// so pseudo "north" vector can be estimated even if the board is not level
+		attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
+		float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
+		float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
+
+		// rotate accels z vector according to roll
+		float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
+		attitudeActual.Pitch = atan2f(accelsData.x, -azn);
+
+		float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
+
+		attitudeActual.Yaw = atan2f(-yn, xn);
+		// TODO: This is still a hack
+		// Put this in a proper generic function in CoordinateConversion.c
+		// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
+		// should calculate the rotation in 3d space using proper cross product math
+		// SUBTODO: formulate the math required
+
+		attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
+		attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
+		attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
 
 		RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
 		AttitudeActualSet(&attitudeActual);
@@ -319,12 +366,12 @@ static int32_t updateAttitudeComplementary(bool first_run)
 		attitudeSettings.AccelKp = 1.0f;
 		attitudeSettings.AccelKi = 0.9f;
 		attitudeSettings.YawBiasRate = 0.23f;
-		magKp = 1;
+		magKp = 1.0f;
 	} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
 		attitudeSettings.AccelKp = 1.0f;
 		attitudeSettings.AccelKi = 0.9f;
 		attitudeSettings.YawBiasRate = 0.23f;
-		magKp = 1;
+		magKp = 1.0f;
 		init = 0;
 	} else if (init == 0) {
 		// Reload settings (all the rates)
@@ -351,8 +398,8 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	quat_copy(&attitudeActual.q1, q);
 
 	// Rotate gravity to body frame and cross with accels
-	grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
+	grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
-	grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
+	grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
 	grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
 	CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
 
@@ -388,13 +435,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
 			brot[2] /= bmag;
 
 			// Only compute if neither vector is null
-			if (bmag < 1 || mag_len < 1)
+			if (bmag < 1.0f || mag_len < 1.0f)
-				mag_err[0] = mag_err[1] = mag_err[2] = 0;
+				mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
 			else
 				CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
 		}
 	} else {
-		mag_err[0] = mag_err[1] = mag_err[2] = 0;
+		mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
 	}
 
 	// Accumulate integral of error.  Scale here so that units are (deg/s) but Ki has units of s
@@ -405,6 +452,13 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	gyrosBias.z -= mag_err[2] * magKi;
 	GyrosBiasSet(&gyrosBias);
 
+	if (revoCalibration.BiasCorrectedRaw != REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
+		// if the raw values are not adjusted, we need to adjust here.
+		gyrosData.x -= gyrosBias.x;
+		gyrosData.y -= gyrosBias.y;
+		gyrosData.z -= gyrosBias.z;
+	}
+
 	// Correct rates based on error, integral component dealt with in updateSensors
 	gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
 	gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
@@ -413,10 +467,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	// Work out time derivative from INSAlgo writeup
 	// Also accounts for the fact that gyros are in deg/s
 	float qdot[4];
-	qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * M_PI_F / 180 / 2;
+	qdot[0] = DEG2RAD(-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT / 2;
-	qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * M_PI_F / 180 / 2;
+	qdot[1] = DEG2RAD(q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT / 2;
-	qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * M_PI_F / 180 / 2;
+	qdot[2] = DEG2RAD(q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT / 2;
-	qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * M_PI_F / 180 / 2;
+	qdot[3] = DEG2RAD(-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT / 2;
 
 	// Take a time step
 	q[0] = q[0] + qdot[0];
@@ -424,7 +478,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	q[2] = q[2] + qdot[2];
 	q[3] = q[3] + qdot[3];
 
-	if(q[0] < 0) {
+	if(q[0] < 0.0f) {
 		q[0] = -q[0];
 		q[1] = -q[1];
 		q[2] = -q[2];
@@ -440,11 +494,11 @@ static int32_t updateAttitudeComplementary(bool first_run)
 
 	// If quaternion has become inappropriately short or is nan reinit.
 	// THIS SHOULD NEVER ACTUALLY HAPPEN
-	if((fabs(qmag) < 1.0e-3f) || isnan(qmag)) {
+	if((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
-		q[0] = 1;
+		q[0] = 1.0f;
-		q[1] = 0;
+		q[1] = 0.0f;
-		q[2] = 0;
+		q[2] = 0.0f;
-		q[3] = 0;
+		q[3] = 0.0f;
 	}
 
 	quat_copy(q, &attitudeActual.q1);
@@ -504,7 +558,12 @@ static int32_t updateAttitudeComplementary(bool first_run)
 	}
 
 
-	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+	if ( variance_error ) {
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
+	} else {
+		AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+	}
+
 
 	return 0;
 }
@@ -538,7 +597,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	static bool gps_updated;
 	static bool gps_vel_updated;
 
-	static float baroOffset = 0;
+	static bool value_error = false;
+
+	static float baroOffset = 0.0f;
 
 	static uint32_t ins_last_time = 0;
 	static bool inited;
@@ -588,8 +649,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
 	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
 	airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
-	gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
+
-	gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
+	// Check if we are running simulation
+	if (!GPSPositionReadOnly()) {
+	    gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
+	} else {
+	    gps_updated |= pdTRUE && outdoor_mode;
+	}
+
+    if (!GPSVelocityReadOnly()) {
+        gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
+    } else {
+        gps_vel_updated |= pdTRUE && outdoor_mode;
+    }
 
 	// Get most recent data
 	GyrosGet(&gyrosData);
@@ -601,119 +673,206 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	GPSVelocityGet(&gpsVelData);
 	GyrosBiasGet(&gyrosBias);
 
-	// Discard mag if it has NAN (normally from bad calibration)
+	value_error = false;
-	mag_updated &= (!isnan(magData.x) && !isinf(magData.x) && !isnan(magData.y) && !isinf(magData.y) && !isnan(magData.z) && !isinf(magData.z));
+	// safety checks
+	if ( invalid(gyrosData.x) ||
+	     invalid(gyrosData.y) ||
+	     invalid(gyrosData.z) ||
+	     invalid(accelsData.x) ||
+	     invalid(accelsData.y) ||
+	     invalid(accelsData.z) ) {
+		// cannot run process update, raise error!
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
+		return 0;
+	}
+	if ( invalid(gyrosBias.x) ||
+	     invalid(gyrosBias.y) ||
+	     invalid(gyrosBias.z) ) {
+		gyrosBias.x = 0.0f;
+		gyrosBias.y = 0.0f;
+		gyrosBias.z = 0.0f;
+	}
+
+	if ( invalid(magData.x) ||
+	     invalid(magData.y) ||
+	     invalid(magData.z) ) {
+
+		// magnetometers can be ignored for a while
+		mag_updated = false;
+		value_error = true;
+	}
+
 	// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
 	// switching between indoor and outdoor mode with Set = false)
-	mag_updated &= (homeLocation.Be[0] > 0.0f || homeLocation.Be[1] > 0.0f || homeLocation.Be[2] > 0.0f);
+	if ( (homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f) ) {
+		mag_updated = false;
+		value_error = true;
+	}
+
+	if ( invalid(baroData.Altitude) ) {
+		baro_updated = false;
+		value_error = true;
+	}
+
+	if ( invalid(airspeedData.CalibratedAirspeed) ) {
+		airspeed_updated = false;
+		value_error = true;
+	}
+
+	if ( invalid(gpsData.Altitude) ) {
+		gps_updated = false;
+		value_error = true;
+	}
+
+	if ( invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) ||
+	     invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) ||
+	     invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) ||
+	     invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) ||
+	     invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) ||
+	     invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN]) ) {
+		gps_updated = false;
+		value_error = true;
+	}
+
+	if ( invalid(gpsVelData.North) ||
+	     invalid(gpsVelData.East) ||
+	     invalid(gpsVelData.Down) ) {
+		gps_vel_updated = false;
+		value_error = true;
+	}
 
 	// Discard airspeed if sensor not connected
-	airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE );
+	if ( airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ) {
+		airspeed_updated = false;
+	}
 
 	// Have a minimum requirement for gps usage
-	gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
+	if ( ( gpsData.Satellites < 7 ) ||
+	     ( gpsData.PDOP > 4.0f ) ||
+	     ( gpsData.Latitude==0 && gpsData.Longitude==0 ) ||
+	     ( homeLocation.Set != HOMELOCATION_SET_TRUE ) ) {
+		gps_updated = false;
+		gps_vel_updated = false;
+	}
 
-	if (!inited)
+	if ( !inited ) {
 		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
-	else if (outdoor_mode && gpsData.Satellites < 7)
+	} else if ( value_error ) {
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
+	} else if ( variance_error ) {
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
+	} else if (outdoor_mode && gpsData.Satellites < 7) {
 		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
-	else
+	} else {
 		AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+	}
 			
-	if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
+	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
+	ins_last_time = PIOS_DELAY_GetRaw();
+
+	// This should only happen at start up or at mode switches
+	if(dT > 0.01f) {
+		dT = 0.01f;
+	} else if(dT <= 0.001f) {
+		dT = 0.001f;
+	}
+
+	if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) {
+
 		// Don't initialize until all sensors are read
-		if (init_stage == 0 && !outdoor_mode) {
+		if (init_stage == 0) {
-			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
+
-			float q[4];
+			// Reset the INS algorithm
+			INSGPSInit();
+			INSSetMagVar( (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
+			                          ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
+			                          ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } );
+			INSSetAccelVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
+			                            ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
+			                            ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } );
+			INSSetGyroVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
+			                            ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
+			                            ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } );
+			INSSetGyroBiasVar( (float[3]){ ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
+			                               ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
+			                               ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } );
+			INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
+
+			// Initialize the gyro bias
+			float gyro_bias[3] = {0.0f, 0.0f, 0.0f};
+			INSSetGyroBias(gyro_bias);
+
 			float pos[3] = {0.0f, 0.0f, 0.0f};
 
-			// Initialize barometric offset to homelocation altitude
+			if (outdoor_mode) {
-			baroOffset = -baroData.Altitude;
-			pos[2] = -(baroData.Altitude + baroOffset);
 
-			// Reset the INS algorithm
+				GPSPositionData gpsPosition;
-			INSGPSInit();
+				GPSPositionGet(&gpsPosition);
-			INSSetMagVar(revoCalibration.mag_var);
-			INSSetAccelVar(revoCalibration.accel_var);
-			INSSetGyroVar(revoCalibration.gyro_var);
-			INSSetBaroVar(revoCalibration.baro_var);
 
-			// Initialize the gyro bias from the settings
+				// Transform the GPS position into NED coordinates
-			float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
+				getNED(&gpsPosition, pos);
-			INSSetGyroBias(gyro_bias);
+
+				// Initialize barometric offset to current GPS NED coordinate
+				baroOffset = -pos[2] - baroData.Altitude;
+
+			} else {
+
+				// Initialize barometric offset to homelocation altitude
+				baroOffset = -baroData.Altitude;
+				pos[2] = -(baroData.Altitude + baroOffset);
+
+			}
 
 			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
 			MagnetometerGet(&magData);
 
-			// Set initial attitude
 			AttitudeActualData attitudeActual;
-			attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
+			AttitudeActualGet (&attitudeActual);
-			attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
+
-			attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
+			// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
+			// so pseudo "north" vector can be estimated even if the board is not level
+			attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
+			float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
+			float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
+
+			// rotate accels z vector according to roll
+			float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
+			attitudeActual.Pitch = atan2f(accelsData.x, -azn);
+
+			float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
+
+			attitudeActual.Yaw = atan2f(-yn, xn);
+			// TODO: This is still a hack
+			// Put this in a proper generic function in CoordinateConversion.c
+			// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
+			// should calculate the rotation in 3d space using proper cross product math
+			// SUBTODO: formulate the math required
+
+			attitudeActual.Roll = RAD2DEG(attitudeActual.Roll);
+			attitudeActual.Pitch = RAD2DEG(attitudeActual.Pitch);
+			attitudeActual.Yaw = RAD2DEG(attitudeActual.Yaw);
+
 			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
 			AttitudeActualSet(&attitudeActual);
 
-			q[0] = attitudeActual.q1;
+			float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 };
-			q[1] = attitudeActual.q2;
-			q[2] = attitudeActual.q3;
-			q[3] = attitudeActual.q4;
 			INSSetState(pos, zeros, q, zeros, zeros);
-			INSResetP(Pdiag);
-		} else if (init_stage == 0 && outdoor_mode) {
-			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
-			float q[4];
-			float NED[3];
-
-			// Reset the INS algorithm
-			INSGPSInit();
-			INSSetMagVar(revoCalibration.mag_var);
-			INSSetAccelVar(revoCalibration.accel_var);
-			INSSetGyroVar(revoCalibration.gyro_var);
-			INSSetBaroVar(revoCalibration.baro_var);
-
-			INSSetMagNorth(homeLocation.Be);
-
-			// Initialize the gyro bias from the settings
-			float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
-			INSSetGyroBias(gyro_bias);
-
-			GPSPositionData gpsPosition;
-			GPSPositionGet(&gpsPosition);
-
-			// Transform the GPS position into NED coordinates
-			getNED(&gpsPosition, NED);
 			
-			// Initialize barometric offset to cirrent GPS NED coordinate
+			INSResetP(ekfConfiguration.P);
-			baroOffset = -NED[2] - baroData.Altitude;
 
-			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
+		} else {
-			MagnetometerGet(&magData);
-
-			// Set initial attitude
-			AttitudeActualData attitudeActual;
-			attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
-			attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
-			attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
-			RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
-			AttitudeActualSet(&attitudeActual);
-
-			q[0] = attitudeActual.q1;
-			q[1] = attitudeActual.q2;
-			q[2] = attitudeActual.q3;
-			q[3] = attitudeActual.q4;
-
-			INSSetState(NED, zeros, q, zeros, zeros);
-			INSResetP(Pdiag);
-		} else if (init_stage > 0) {
 			// Run prediction a bit before any corrections
-			dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
 
-			GyrosBiasGet(&gyrosBias);
+			// Because the sensor module remove the bias we need to add it
-			float gyros[3] = {(gyrosData.x + gyrosBias.x) * M_PI_F / 180.0f,
+			// back in here so that the INS algorithm can track it correctly
-				(gyrosData.y + gyrosBias.y) * M_PI_F / 180.0f,
+			float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
-				(gyrosData.z + gyrosBias.z) * M_PI_F / 180.0f};
+			if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
+				gyros[0] += DEG2RAD(gyrosBias.x);
+				gyros[1] += DEG2RAD(gyrosBias.y);
+				gyros[2] += DEG2RAD(gyrosBias.z);
+			}
 			INSStatePrediction(gyros, &accelsData.x, dT);
-			
+
 			AttitudeActualData attitude;
 			AttitudeActualGet(&attitude);
 			attitude.q1 = Nav.q[0];
@@ -728,38 +887,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		if(init_stage > 10)
 			inited = true;
 
-		ins_last_time = PIOS_DELAY_GetRaw();	
-
 		return 0;
 	}
 
 	if (!inited)
 		return 0;
 
-	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
-	ins_last_time = PIOS_DELAY_GetRaw();
-
-	// This should only happen at start up or at mode switches
-	if(dT > 0.01f)
-		dT = 0.01f;
-	else if(dT <= 0.001f)
-		dT = 0.001f;
-
-	// If the gyro bias setting was updated we should reset
-	// the state estimate of the EKF
-	if(gyroBiasSettingsUpdated) {
-		float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
-		INSSetGyroBias(gyro_bias);
-		gyroBiasSettingsUpdated = false;
-	}
-
 	// Because the sensor module remove the bias we need to add it
 	// back in here so that the INS algorithm can track it correctly
-	float gyros[3] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f};
+	float gyros[3] = { DEG2RAD(gyrosData.x), DEG2RAD(gyrosData.y), DEG2RAD(gyrosData.z) };
 	if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
-		gyros[0] += gyrosBias.x * M_PI_F / 180.0f;
+		gyros[0] += DEG2RAD(gyrosBias.x);
-		gyros[1] += gyrosBias.y * M_PI_F / 180.0f;
+		gyros[1] += DEG2RAD(gyrosBias.y);
-		gyros[2] += gyrosBias.z * M_PI_F / 180.0f;
+		gyros[2] += DEG2RAD(gyrosBias.z);
 	}
 
 	// Advance the state estimate
@@ -788,14 +928,19 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	
 	if (gps_updated && outdoor_mode)
 	{
-		INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
+		INSSetPosVelVar((float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
+		                            ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
+		                            ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
+		                (float[3]){ ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
+		                            ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
+		                            ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] });
 		sensors |= POS_SENSORS;
 
 		if (0) { // Old code to take horizontal velocity from GPS Position update
 			sensors |= HORIZ_SENSORS;
 			vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
 			vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
-			vel[2] = 0;
+			vel[2] = 0.0f;
 		}
 		// Transform the GPS position into NED coordinates
 		getNED(&gpsData, NED);
@@ -806,9 +951,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		    * ( -NED[2] - baroData.Altitude );
 
 	} else if (!outdoor_mode) {
-		INSSetPosVelVar(1e6f, 1e5f);
+		INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
-		vel[0] = vel[1] = vel[2] = 0;
+		                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
-		NED[0] = NED[1] = 0;
+		                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
+		                (float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
+		                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
+		                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] });
+		vel[0] = vel[1] = vel[2] = 0.0f;
+		NED[0] = NED[1] = 0.0f;
 		NED[2] = -(baroData.Altitude + baroOffset);
 		sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
 		sensors |= POS_SENSORS |VERT_SENSORS;
@@ -833,11 +983,16 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		if ( !gps_vel_updated && !gps_updated) {
 			// feed airspeed into EKF, treat wind as 1e2 variance
 			sensors |= HORIZ_SENSORS | VERT_SENSORS;
-			INSSetPosVelVar(1e6f, 1e2f);
+			INSSetPosVelVar((float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
+			                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
+			                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
+			                (float[3]){ ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
+			                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
+			                            ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] });
 			// rotate airspeed vector into NED frame - airspeed is measured in X axis only
 			float R[3][3];
 			Quaternion2R(Nav.q,R);
-			float vtas[3] = {airspeed.TrueAirspeed,0,0};
+			float vtas[3] = {airspeed.TrueAirspeed,0.0f,0.0f};
 			rot_mult(R,vtas,vel);
 		}
 	}
@@ -864,15 +1019,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	velocityActual.Down = Nav.Vel[2];
 	VelocityActualSet(&velocityActual);
 
-	if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
+	gyrosBias.x = RAD2DEG(Nav.gyro_bias[0]);
-		// Copy the gyro bias into the UAVO except when it was updated
+	gyrosBias.y = RAD2DEG(Nav.gyro_bias[1]);
-		// from the settings during the calculation, then consume it
+	gyrosBias.z = RAD2DEG(Nav.gyro_bias[2]);
-		// next cycle
+	GyrosBiasSet(&gyrosBias);
-		gyrosBias.x = Nav.gyro_bias[0] * 180.0f / M_PI_F;
+
-		gyrosBias.y = Nav.gyro_bias[1] * 180.0f / M_PI_F;
+	EKFStateVarianceData vardata;
-		gyrosBias.z = Nav.gyro_bias[2] * 180.0f / M_PI_F;
+	EKFStateVarianceGet(&vardata);
-		GyrosBiasSet(&gyrosBias);
+	INSGetP(vardata.P);
-	}
+	EKFStateVarianceSet(&vardata);
 
 	return 0;
 }
@@ -902,24 +1057,58 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
 
 static void settingsUpdatedCb(UAVObjEvent * ev) 
 {
+	if (ev == NULL || ev->obj == FlightStatusHandle()) {
+		FlightStatusGet(&flightStatus);
+	}
 	if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
 		RevoCalibrationGet(&revoCalibration);
-		
+	}
-		/* When the revo calibration is updated, update the GyroBias object */
+	// change of these settings require reinitialization of the EKF
-		GyrosBiasData gyrosBias;
+	// when an error flag has been risen, we also listen to flightStatus updates,
-		GyrosBiasGet(&gyrosBias);
+	// since we are waiting for the system to get disarmed so we can reinitialize safely.
-		gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
+	if (ev == NULL ||
-		gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
+			ev->obj == EKFConfigurationHandle() ||
-		gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
+			ev->obj == RevoSettingsHandle() ||
-		GyrosBiasSet(&gyrosBias);
+			( variance_error==true && ev->obj == FlightStatusHandle() )
+			) {
 
-		gyroBiasSettingsUpdated = true;
+		bool error = false;
 
-		// In case INS currently running
+		EKFConfigurationGet(&ekfConfiguration);
-		INSSetMagVar(revoCalibration.mag_var);
+		int t;
-		INSSetAccelVar(revoCalibration.accel_var);
+		for (t=0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
-		INSSetGyroVar(revoCalibration.gyro_var);
+			if (invalid_var(ekfConfiguration.P[t])) {
-		INSSetBaroVar(revoCalibration.baro_var);
+				error = true;
+			}
+		}
+		for (t=0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
+			if (invalid_var(ekfConfiguration.Q[t])) {
+				error = true;
+			}
+		}
+		for (t=0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
+			if (invalid_var(ekfConfiguration.R[t])) {
+				error = true;
+			}
+		}
+
+		RevoSettingsGet(&revoSettings);
+
+		// Reinitialization of the EKF is not desired during flight.
+		// It will be delayed until the board is disarmed by raising the error flag.
+		// We will not prevent the initial initialization though, since the board could be in always armed mode.
+		if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required ) {
+			error = true;
+		}
+
+		if (error) {
+			variance_error = true;
+		} else {
+			// trigger reinitialization - possibly with new algorithm
+			running_algorithm = revoSettings.FusionAlgorithm;
+			variance_error = false;
+			initialization_required = true;
+		}
 	}
 	if(ev == NULL || ev->obj == HomeLocationHandle()) {
 		HomeLocationGet(&homeLocation);
@@ -931,11 +1120,12 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
 		T[0] = alt+6.378137E6f;
 		T[1] = cosf(lat)*(alt+6.378137E6f);
 		T[2] = -1.0f;
+
+		// TODO: convert positionActual to new reference frame and gracefully update EKF state!
+		// needed for long range flights where the reference coordinate is adjusted in flight
 	}
 	if (ev == NULL || ev->obj == AttitudeSettingsHandle())
 		AttitudeSettingsGet(&attitudeSettings);
-	if (ev == NULL || ev->obj == RevoSettingsHandle())
-		RevoSettingsGet(&revoSettings);
 }
 /**
  * @}

flight/modules/Autotune/autotune.c

@@ -120,7 +120,7 @@ MODULE_INITCALL(AutotuneInitialize, AutotuneStart)
 /**
  * Module thread, should not return.
  */
-static void AutotuneTask(void *parameters)
+static void AutotuneTask(__attribute__((unused)) void *parameters)
 {
 	//AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 	

flight/modules/Battery/battery.c

@@ -124,7 +124,7 @@ int32_t BatteryInitialize(void)
 
 MODULE_INITCALL(BatteryInitialize, 0)
 #define HAS_SENSOR(x) batterySettings.SensorType[x]==FLIGHTBATTERYSETTINGS_SENSORTYPE_ENABLED 
-static void onTimer(UAVObjEvent* ev)
+static void onTimer(__attribute__((unused)) UAVObjEvent* ev)
 {
 	static FlightBatteryStateData flightBatteryData;
 	FlightBatterySettingsData batterySettings;

flight/modules/CallbackTest/callbacktest.c

@@ -0,0 +1,132 @@
+/**
+ ******************************************************************************
+ *
+ * @file       callbacktest.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Example module to be used as a template for actual modules.
+ *             Event callback version.
+ *
+ * @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
+ */
+
+/**
+ *
+ * This is a test suite to test the callback scheduler,
+ * including its forward scheduling ability
+ *
+ */
+
+#include "openpilot.h"
+
+// Private constants
+#define STACK_SIZE configMINIMAL_STACK_SIZE
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
+#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY
+// Private types
+
+//#define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__)
+#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__ ); xSemaphoreGiveRecursive(mutex);
+
+static xSemaphoreHandle mutex;
+
+// Private variables
+static DelayedCallbackInfo *cbinfo[10];
+
+static volatile int32_t counter[10]={0};
+
+static void DelayedCb0();
+static void DelayedCb1();
+static void DelayedCb2();
+static void DelayedCb3();
+static void DelayedCb4();
+static void DelayedCb5();
+static void DelayedCb6();
+/**
+ * Initialise the module, called on startup.
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t CallbackTestInitialize()
+{
+
+	mutex = xSemaphoreCreateRecursiveMutex();
+
+	cbinfo[0] = DelayedCallbackCreate(&DelayedCb0,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[1] = DelayedCallbackCreate(&DelayedCb1,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[2] = DelayedCallbackCreate(&DelayedCb2,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[3] = DelayedCallbackCreate(&DelayedCb3,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[4] = DelayedCallbackCreate(&DelayedCb4,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[5] = DelayedCallbackCreate(&DelayedCb5,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[6] = DelayedCallbackCreate(&DelayedCb6,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+20,STACK_SIZE);
+
+	
+	return 0;
+}
+int32_t CallbackTestStart() {
+	
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	DelayedCallbackDispatch(cbinfo[3]);
+	DelayedCallbackDispatch(cbinfo[2]);
+	DelayedCallbackDispatch(cbinfo[1]);
+	DelayedCallbackDispatch(cbinfo[0]);
+	// different callback priorities within a taskpriority
+	DelayedCallbackSchedule(cbinfo[4],30000,CALLBACK_UPDATEMODE_NONE);
+	DelayedCallbackSchedule(cbinfo[4],5000,CALLBACK_UPDATEMODE_OVERRIDE);
+	DelayedCallbackSchedule(cbinfo[4],4000,CALLBACK_UPDATEMODE_SOONER);
+	DelayedCallbackSchedule(cbinfo[4],10000,CALLBACK_UPDATEMODE_SOONER);
+	DelayedCallbackSchedule(cbinfo[4],1000,CALLBACK_UPDATEMODE_LATER);
+	DelayedCallbackSchedule(cbinfo[4],4800,CALLBACK_UPDATEMODE_LATER);
+	DelayedCallbackSchedule(cbinfo[4],48000,CALLBACK_UPDATEMODE_NONE);
+	// should be at 4.8 seconds after this, allowing for exactly 9 prints of the following
+	DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE);
+	// delayed counter with 500 ms
+	DelayedCallbackDispatch(cbinfo[6]);
+	// high task prio
+	xSemaphoreGiveRecursive(mutex);
+	return 0;
+}
+
+static void DelayedCb0() {
+	DEBUGPRINT("delayed counter low prio 0 updated: %i\n",counter[0]);
+	if (++counter[0]<10) DelayedCallbackDispatch(cbinfo[0]);
+}
+static void DelayedCb1() {
+	DEBUGPRINT("delayed counter low prio 1 updated: %i\n",counter[1]);
+	if (++counter[1]<10) DelayedCallbackDispatch(cbinfo[1]);
+}
+static void DelayedCb2() {
+	DEBUGPRINT("delayed counter high prio 2 updated: %i\n",counter[2]);
+	if (++counter[2]<10) DelayedCallbackDispatch(cbinfo[2]);
+}
+static void DelayedCb3() {
+	DEBUGPRINT("delayed counter high prio 3 updated: %i\n",counter[3]);
+	if (++counter[3]<10) DelayedCallbackDispatch(cbinfo[3]);
+}
+static void DelayedCb4() {
+	DEBUGPRINT("delayed scheduled callback 4 reached!\n");
+	exit(0);
+}
+static void DelayedCb5() {
+	DEBUGPRINT("delayed scheduled counter 5 updated: %i\n",counter[5]);
+	if (++counter[5]<10) DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE);
+	// it will likely only reach 8 due to cb4 aborting the run
+}
+static void DelayedCb6() {
+	DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n",counter[6]);
+	if (++counter[6]<10) DelayedCallbackDispatch(cbinfo[6]);
+}

flight/modules/CameraStab/camerastab.c

@@ -180,7 +180,7 @@ static void attitudeUpdated(UAVObjEvent* ev)
 					break;
 				case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
 					input_rate = accessory.AccessoryVal * cameraStab.InputRate[i];
-					if (fabs(input_rate) > cameraStab.MaxAxisLockRate)
+					if (fabsf(input_rate) > cameraStab.MaxAxisLockRate)
 						csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]);
 					break;
 				default:
@@ -328,7 +328,7 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
 	float delta = *attitude - csd->ffLastAttitudeFiltered[index];
 	float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis;
 
-	if (fabs(delta) > maxDelta) {
+	if (fabsf(delta) > maxDelta) {
 		// we are accelerating too hard
 		*attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta);
 	}

flight/modules/ComUsbBridge/ComUsbBridge.c

@@ -128,7 +128,7 @@ MODULE_INITCALL(comUsbBridgeInitialize, comUsbBridgeStart)
  * Main task. It does not return.
  */
 
-static void com2UsbBridgeTask(void *parameters)
+static void com2UsbBridgeTask(__attribute__((unused)) void *parameters)
 {
 	/* Handle usart -> vcp direction */
 	volatile uint32_t tx_errors = 0;
@@ -138,7 +138,7 @@ static void com2UsbBridgeTask(void *parameters)
 		rx_bytes = PIOS_COM_ReceiveBuffer(usart_port, com2usb_buf, BRIDGE_BUF_LEN, 500);
 		if (rx_bytes > 0) {
 			/* Bytes available to transfer */
-			if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != rx_bytes) {
+			if (PIOS_COM_SendBuffer(vcp_port, com2usb_buf, rx_bytes) != (int32_t)rx_bytes) {
 				/* Error on transmit */
 				tx_errors++;
 			}
@@ -146,7 +146,7 @@ static void com2UsbBridgeTask(void *parameters)
 	}
 }
 
-static void usb2ComBridgeTask(void * parameters)
+static void usb2ComBridgeTask(__attribute__((unused)) void * parameters)
 {
 	/* Handle vcp -> usart direction */
 	volatile uint32_t tx_errors = 0;
@@ -156,7 +156,7 @@ static void usb2ComBridgeTask(void * parameters)
 		rx_bytes = PIOS_COM_ReceiveBuffer(vcp_port, usb2com_buf, BRIDGE_BUF_LEN, 500);
 		if (rx_bytes > 0) {
 			/* Bytes available to transfer */
-			if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != rx_bytes) {
+			if (PIOS_COM_SendBuffer(usart_port, usb2com_buf, rx_bytes) != (int32_t)rx_bytes) {
 				/* Error on transmit */
 				tx_errors++;
 			}

flight/modules/Example/examplemodcallback.c

@@ -0,0 +1,140 @@
+/**
+ ******************************************************************************
+ *
+ * @file       examplemodcallback.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Example module to be used as a template for actual modules.
+ *             Event callback version.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Input objects: ExampleObject1, ExampleSettings
+ * Output object: ExampleObject2
+ *
+ * This module executes in response to ExampleObject1 updates. When the
+ * module is triggered it will update the data of ExampleObject2.
+ *
+ * No threads are used in this example.
+ *
+ * UAVObjects are automatically generated by the UAVObjectGenerator from
+ * the object definition XML file.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+#include "exampleobject1.h"	// object the module will listen for updates (input)
+#include "exampleobject2.h"	// object the module will update (output)
+#include "examplesettings.h"	// object holding module settings (input)
+
+// Private constants
+#define STACK_SIZE configMINIMAL_STACK_SIZE
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
+#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY
+// Private types
+
+// Private variables
+static DelayedCallbackInfo *cbinfo;
+
+// Private functions
+static void ObjectUpdatedCb(UAVObjEvent *ev);
+
+static void DelayedCb();
+/**
+ * Initialise the module, called on startup.
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t ExampleModCallbackInitialize()
+{
+	// Listen for ExampleObject1 updates, connect a callback function
+	ExampleObject1ConnectCallback(&ObjectUpdatedCb);
+
+	cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE);
+	
+	return 0;
+}
+
+/**
+ * This function is called each time ExampleObject1 is updated, this could be
+ * a local update or a remote update from the GCS. In this example the module
+ * does not have its own thread, the callbacks are executed from within the
+ * event thread. Because of that the callback execution time must be kept to
+ * a minimum.
+ */
+static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+	DelayedCallbackDispatch(cbinfo);
+}
+
+/**
+ * This function is called by the DelayedCallbackScheduler when its execution
+ * has been requested.  Callbacks scheduled for execution are executed in the
+ * same thread in a round robin fashion. The Dispatch function to reschedule
+ * execution can be called from within the Callback itself, in which case the
+ * re-run will be scheduled after all other callback with equal or higher
+ * priority have been executed.  Like event callbacks, delayed callbacks are
+ * executed in the same thread context one at a time, therefore blocking IO
+ * functions or very long lasting calculations are prohibited. Unlike Event
+ * callbacks these callbacks run with a standard (IDLE+1) thread priority and
+ * do not block regular threads. They are therefore saver to use.
+ */
+static void DelayedCb();
+	ExampleSettingsData settings;
+	ExampleObject1Data data1;
+	ExampleObject2Data data2;
+	int32_t step;
+
+	// Update settings with latest value
+	ExampleSettingsGet(&settings);
+
+	// Get the input object
+	ExampleObject1Get(&data1);
+
+	// Determine how to update the output object
+	if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) {
+		step = settings.StepSize;
+	} else {
+		step = -settings.StepSize;
+	}
+
+	// Update data
+	data2.Field1 = data1.Field1 + step;
+	data2.Field2 = data1.Field2 + step;
+	data2.Field3 = data1.Field3 + step;
+	data2.Field4[0] = data1.Field4[0] + step;
+	data2.Field4[1] = data1.Field4[1] + step;
+
+	// Update the ExampleObject2, after this function is called
+	// notifications to any other modules listening to that object
+	// will be sent and the GCS object will be updated through the
+	// telemetry link. All operations will take place asynchronously
+	// and the following call will return immediately.
+	ExampleObject2Set(&data2);
+
+	//call the module again 10 seconds later,
+	//even if the exampleobject has not been updated
+	DelayedCallbackSchedule(cbinfo, 10*1000, CALLBACK_UPDATEMODE_NONE);
+	
+}

flight/modules/Example/examplemodperiodic.c

@@ -76,7 +76,7 @@ int32_t ExampleModPeriodicInitialize()
 /**
  * Module thread, should not return.
  */
-static void exampleTask(void *parameters)
+static void exampleTask(__attribute__((unused)) void *parameters)
 {
 	ExampleSettingsData settings;
 	ExampleObject2Data data;

flight/modules/Example/examplemodthread.c

@@ -84,7 +84,7 @@ int32_t ExampleModThreadInitialize()
 /**
  * Module thread, should not return.
  */
-static void exampleTask(void *parameters)
+static void exampleTask(__attribute__((unused)) void *parameters)
 {
 	UAVObjEvent ev;
 	ExampleSettingsData settings;

flight/modules/Extensions/MagBaro/magbaro.c

@@ -45,9 +45,9 @@
 #include "taskinfo.h"
 
 // Private constants
-#define STACK_SIZE_BYTES 620
+#define STACK_SIZE_BYTES 600
 #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
-#define UPDATE_PERIOD 50
+#define UPDATE_PERIOD 100
 
 // Private types
 
@@ -55,14 +55,20 @@
 static xTaskHandle taskHandle;
 
 // down sampling variables
+static bool magbaroEnabled;
+
+#if defined(PIOS_INCLUDE_BMP085)
 #define alt_ds_size    4
 static int32_t alt_ds_temp = 0;
 static int32_t alt_ds_pres = 0;
 static int alt_ds_count = 0;
+#endif
+
+#if defined(PIOS_INCLUDE_HMC5883)
 int32_t mag_test;
-static bool magbaroEnabled;
 static float mag_bias[3] = {0,0,0};
 static float mag_scale[3] = {1,1,1};
+#endif
 
         // Private functions
 static void magbaroTask(void *parameters);
@@ -77,7 +83,7 @@ int32_t MagBaroStart()
 	if (magbaroEnabled) {
 		// Start main task
 		xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
-		//PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle);
+		PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MAGBARO, taskHandle);
 		return 0;
 	}
 	return -1;
@@ -104,13 +110,18 @@ int32_t MagBaroInitialize()
 
 	if(magbaroEnabled)
 	{
+#if defined(PIOS_INCLUDE_HMC5883)
 		MagnetometerInitialize();
+#endif
+
+#if defined(PIOS_INCLUDE_BMP085)
 		BaroAltitudeInitialize();
 
 		// init down-sampling data
 		alt_ds_temp = 0;
 		alt_ds_pres = 0;
 		alt_ds_count = 0;
+#endif
 	}
 	return 0;
 }
@@ -118,7 +129,7 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart)
 /**
  * Module thread, should not return.
  */
-
+#if defined(PIOS_INCLUDE_HMC5883)
 static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
 #ifdef PIOS_HMC5883_HAS_GPIOS
 	.exti_cfg = 0,
@@ -129,27 +140,25 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
 	.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
 
 };
+#endif
 
-static void magbaroTask(void *parameters)
+static void magbaroTask(__attribute__((unused)) void *parameters)
 {
-	BaroAltitudeData data;
 	portTickType lastSysTime;
 	
 #if defined(PIOS_INCLUDE_BMP085)
+	BaroAltitudeData data;
 	PIOS_BMP085_Init();
 #endif
-#if defined(PIOS_INCLUDE_HMC5883)
-	PIOS_HMC5883_Init(&pios_hmc5883_cfg);
-#endif
 
 #if defined(PIOS_INCLUDE_HMC5883)
-	//mag_test = PIOS_HMC5883_Test();
+	MagnetometerData mag;
-#else
+	PIOS_HMC5883_Init(&pios_hmc5883_cfg);
-	mag_test = 0;
+	uint32_t mag_update_time = PIOS_DELAY_GetRaw();
 #endif
+
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
-	uint32_t mag_update_time = PIOS_DELAY_GetRaw();
 	while (1)
 	{
 #if defined(PIOS_INCLUDE_BMP085)
@@ -162,7 +171,7 @@ static void magbaroTask(void *parameters)
 #endif
 		PIOS_BMP085_ReadADC();
 		alt_ds_temp += PIOS_BMP085_GetTemperature();
-		
+
 		// Update the pressure data
 		PIOS_BMP085_StartADC(PressureConv);
 #ifdef PIOS_BMP085_HAS_GPIOS
@@ -172,7 +181,7 @@ static void magbaroTask(void *parameters)
 #endif
 		PIOS_BMP085_ReadADC();
 		alt_ds_pres += PIOS_BMP085_GetPressure();
-		
+
 		if (++alt_ds_count >= alt_ds_size)
 		{
 			alt_ds_count = 0;
@@ -194,7 +203,6 @@ static void magbaroTask(void *parameters)
 #endif
 
 #if defined(PIOS_INCLUDE_HMC5883)
-		MagnetometerData mag;
 		if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
 			int16_t values[3];
 			PIOS_HMC5883_ReadMag(values);

flight/modules/FirmwareIAP/firmwareiap.c

@@ -238,7 +238,7 @@ static uint32_t get_time(void)
 /**
  * Executed by event dispatcher callback to reset INS before resetting OP 
  */
-static void resetTask(UAVObjEvent * ev)
+static void resetTask(__attribute__((unused)) UAVObjEvent * ev)
 {
 #if defined (PIOS_LED_HEARTBEAT)
 	PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);

flight/modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -144,7 +144,7 @@ static float indicatedAirspeedActualBias = 0;
 /**
  * Module thread, should not return.
  */
-static void pathfollowerTask(void *parameters)
+static void pathfollowerTask(__attribute__((unused)) void *parameters)
 {
 	SystemSettingsData systemSettings;
 	FlightStatusData flightStatus;
@@ -316,7 +316,7 @@ static void updatePathVelocity()
 	if (angle1>180.0f) angle1-=360.0f;
 	if (angle2<-180.0f) angle2+=360.0f;
 	if (angle2>180.0f) angle2-=360.0f;
-	if (fabs(angle1)>=90.0f && fabs(angle2)>=90.0f) {
+	if (fabsf(angle1)>=90.0f && fabsf(angle2)>=90.0f) {
 		error_speed=0;
 	}
 
@@ -630,13 +630,13 @@ static float bound(float val, float min, float max)
 	return val;
 }
 
-static void SettingsUpdatedCb(UAVObjEvent * ev)
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
 	PathDesiredGet(&pathDesired);
 }
 
-static void airspeedActualUpdatedCb(UAVObjEvent * ev)
+static void airspeedActualUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 
 	AirspeedActualData airspeedActual;

flight/modules/FlightPlan/flightplan.c

@@ -97,7 +97,7 @@ MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart)
 /**
  * Module task
  */
-static void flightPlanTask(void *parameters)
+static void flightPlanTask(__attribute__((unused)) void *parameters)
 {
 	UAVObjEvent ev;
 	PmReturn_t retval;

flight/modules/FlightPlan/lib/uavobject.pyt.template

@@ -30,31 +30,21 @@
 # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 #
 
-
 from uavobject import *
 
 $(DATAFIELDS)
 
 # Object $(NAME) definition
 class $(NAME)(UAVObject):
-	# Object constants
+    # Object constants
-	OBJID = $(UOBJID)
+    OBJID = $(UOBJID)
 
-	# Constructor
+    # Constructor
-	def __init__(self):
+    def __init__(self):
-		UAVObject.__init__(self, $(NAME).OBJID)
+        UAVObject.__init__(self, $(NAME).OBJID)
 
-		# Create object fields
+        # Create object fields
 $(DATAFIELDINIT)
-		# Read field data
+        # Read field data
-		self.read()
+        self.read()
-		self.metadata.read()
+        self.metadata.read()
-
-
-
-
-
-
-
-
-

flight/modules/GPS/GPS.c

@@ -196,7 +196,7 @@ MODULE_INITCALL(GPSInitialize, GPSStart)
  * Main gps task. It does not return.
  */
 
-static void gpsTask(void *parameters)
+static void gpsTask(__attribute__((unused)) void *parameters)
 {
 	portTickType xDelay = 100 / portTICK_RATE_MS;
 	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
@@ -277,7 +277,7 @@ static void gpsTask(void *parameters)
 /*
  * Estimate the acceleration due to gravity for a particular location in LLA
  */
-static float GravityAccel(float latitude, float longitude, float altitude)
+static float GravityAccel(float latitude, __attribute__((unused)) float longitude, float altitude)
 {
 	/* WGS84 gravity model.  The effect of gravity over latitude is strong
 	 * enough to change the estimated accelerometer bias in those apps. */

flight/modules/GPS/NMEA.c

@@ -78,7 +78,7 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
 #endif //PIOS_GPS_MINIMAL
 
-const static struct nmea_parser nmea_parsers[] = {
+static const struct nmea_parser nmea_parsers[] = {
 	{
 		.prefix = "GPGGA",
 		.handler = nmeaProcessGPGGA,
@@ -189,7 +189,7 @@ int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData,
 	return PARSER_INCOMPLETE;
 }
 
-const static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
+static const struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
 {
 	if (!prefix) {
 		return (NULL);
@@ -601,7 +601,7 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
  * \param[in] A pointer to a GPSPosition UAVObject to be updated (unused).
  * \param[in] An NMEA sentence with a valid checksum
  */
-static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
+static bool nmeaProcessGPZDA(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
 {
 	if (nbParam != 7)
 		return false;
@@ -640,7 +640,7 @@ static uint8_t gsv_processed_mask;
 static uint16_t gsv_incomplete_error;
 static uint16_t gsv_duplicate_error;
 
-static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
+static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
 {
 	if (nbParam < 4)
 		return false;

flight/modules/MK/MKSerial/MKSerial.c

@@ -501,7 +501,7 @@ static void DoConnectedToNC(void)
 	}
 }
 
-static void MkSerialTask(void *parameters)
+static void MkSerialTask(__attribute__((unused)) void *parameters)
 {
 	MkMsg_t msg;
 	uint32_t version;

flight/modules/ManualControl/inc/manualcontrol.h

@@ -42,9 +42,12 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
 	(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
-	(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
-	(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : FLIGHTMODE_UNDEFINED \
+	(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
+	(x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \
+	(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \
+	(x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \
+	FLIGHTMODE_UNDEFINED \
 	)
 
 int32_t ManualControlInitialize();
@@ -105,8 +108,14 @@ int32_t ManualControlInitialize();
 ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
 ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
 ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \
 ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
-( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int) FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int) FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int) FLIGHTSTATUS_FLIGHTMODE_LAND) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int) FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \
+( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int) FLIGHTSTATUS_FLIGHTMODE_POI) \
 )
 
 #define assumptions_channelcount ( \

flight/modules/ManualControl/manualcontrol.c

@@ -95,6 +95,7 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
 // Private functions
 static void updateActuatorDesired(ManualControlCommandData * cmd);
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
+static void updateLandDesired(ManualControlCommandData * cmd, bool changed);
 static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed);
 static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home);
 static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
@@ -115,10 +116,11 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
 
 #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
 #define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
-struct rcvr_activity_fsm {
+struct rcvr_activity_fsm
-	ManualControlSettingsChannelGroupsOptions group;
+{
-	uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
+    ManualControlSettingsChannelGroupsOptions group;
-	uint8_t sample_count;
+    uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
+    uint8_t sample_count;
 };
 static struct rcvr_activity_fsm activity_fsm;
 
@@ -132,12 +134,12 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
  */
 int32_t ManualControlStart()
 {
-	// Start main task
+    // Start main task
-	xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
+    xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
-	PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
+    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
-	PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
+    PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
 
-	return 0;
+    return 0;
 }
 
 /**
@@ -146,548 +148,552 @@ int32_t ManualControlStart()
 int32_t ManualControlInitialize()
 {
 
-	/* Check the assumptions about uavobject enum's are correct */
+    /* Check the assumptions about uavobject enum's are correct */
-	if(!assumptions)
+    if (!assumptions)
-		return -1;
+        return -1;
 
-	AccessoryDesiredInitialize();
+    AccessoryDesiredInitialize();
-	ManualControlCommandInitialize();
+    ManualControlCommandInitialize();
-	FlightStatusInitialize();
+    FlightStatusInitialize();
-	StabilizationDesiredInitialize();
+    StabilizationDesiredInitialize();
-	ReceiverActivityInitialize();
+    ReceiverActivityInitialize();
-	ManualControlSettingsInitialize();
+    ManualControlSettingsInitialize();
 
-	return 0;
+    return 0;
 }
-MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
+MODULE_INITCALL( ManualControlInitialize, ManualControlStart)
 
 /**
  * Module task
  */
-static void manualControlTask(void *parameters)
+static void manualControlTask(__attribute__((unused)) void *parameters)
 {
-	ManualControlSettingsData settings;
+    ManualControlSettingsData settings;
-	ManualControlCommandData cmd;
+    ManualControlCommandData cmd;
-	FlightStatusData flightStatus;
+    FlightStatusData flightStatus;
-	float flightMode = 0;
+    float flightMode = 0;
 
-	uint8_t disconnected_count = 0;
+    uint8_t disconnected_count = 0;
-	uint8_t connected_count = 0;
+    uint8_t connected_count = 0;
 
-	// For now manual instantiate extra instances of Accessory Desired.  In future should be done dynamically
+    // For now manual instantiate extra instances of Accessory Desired.  In future should be done dynamically
-	// this includes not even registering it if not used
+    // this includes not even registering it if not used
-	AccessoryDesiredCreateInstance();
+    AccessoryDesiredCreateInstance();
-	AccessoryDesiredCreateInstance();
+    AccessoryDesiredCreateInstance();
 
-	// Run this initially to make sure the configuration is checked
+    // Run this initially to make sure the configuration is checked
-	configuration_check();
+    configuration_check();
-	
-	// Whenever the configuration changes, make sure it is safe to fly
-	SystemSettingsConnectCallback(configurationUpdatedCb);
-	ManualControlSettingsConnectCallback(configurationUpdatedCb);
-	
-	// Whenever the configuration changes, make sure it is safe to fly
 
-	// Make sure unarmed on power up
+    // Whenever the configuration changes, make sure it is safe to fly
-	ManualControlCommandGet(&cmd);
+    SystemSettingsConnectCallback(configurationUpdatedCb);
-	FlightStatusGet(&flightStatus);
+    ManualControlSettingsConnectCallback(configurationUpdatedCb);
-	flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
-	armState = ARM_STATE_DISARMED;
 
-	/* Initialize the RcvrActivty FSM */
+    // Whenever the configuration changes, make sure it is safe to fly
-	portTickType lastActivityTime = xTaskGetTickCount();
-	resetRcvrActivity(&activity_fsm);
 
-	// Main task loop
+    // Make sure unarmed on power up
-	lastSysTime = xTaskGetTickCount();
+    ManualControlCommandGet(&cmd);
-	while (1) {
+    FlightStatusGet(&flightStatus);
-		float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
+    flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
+    armState = ARM_STATE_DISARMED;
 
-		// Wait until next update
+    /* Initialize the RcvrActivty FSM */
-		vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
+    portTickType lastActivityTime = xTaskGetTickCount();
-		PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
+    resetRcvrActivity(&activity_fsm);
 
-		// Read settings
+    // Main task loop
-		ManualControlSettingsGet(&settings);
+    lastSysTime = xTaskGetTickCount();
+    while (1) {
+        float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
 
-		/* Update channel activity monitor */
+        // Wait until next update
-		if (flightStatus.Armed == ARM_STATE_DISARMED) {
+        vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
-			if (updateRcvrActivity(&activity_fsm)) {
+        PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
-				/* Reset the aging timer because activity was detected */
-				lastActivityTime = lastSysTime;
-			}
-		}
-		if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
-			resetRcvrActivity(&activity_fsm);
-			lastActivityTime = lastSysTime;
-		}
 
-		if (ManualControlCommandReadOnly()) {
+        // Read settings
-			FlightTelemetryStatsData flightTelemStats;
+        ManualControlSettingsGet(&settings);
-			FlightTelemetryStatsGet(&flightTelemStats);
-			if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
-				/* trying to fly via GCS and lost connection.  fall back to transmitter */
-				UAVObjMetadata metadata;
-				ManualControlCommandGetMetadata(&metadata);
-				UAVObjSetAccess(&metadata, ACCESS_READWRITE);
-				ManualControlCommandSetMetadata(&metadata);
-			}
-		}
 
-		if (!ManualControlCommandReadOnly()) {
+        /* Update channel activity monitor */
+        if (flightStatus.Armed == ARM_STATE_DISARMED) {
+            if (updateRcvrActivity(&activity_fsm)) {
+                /* Reset the aging timer because activity was detected */
+                lastActivityTime = lastSysTime;
+            }
+        }
+        if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
+            resetRcvrActivity(&activity_fsm);
+            lastActivityTime = lastSysTime;
+        }
 
-			bool valid_input_detected = true;
+        if (ManualControlCommandReadOnly()) {
-			
+            FlightTelemetryStatsData flightTelemStats;
-			// Read channel values in us
+            FlightTelemetryStatsGet(&flightTelemStats);
-			for (uint8_t n = 0; 
+            if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
-			     n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
+                /* trying to fly via GCS and lost connection.  fall back to transmitter */
-			     ++n) {
+                UAVObjMetadata metadata;
-				extern uint32_t pios_rcvr_group_map[];
+                ManualControlCommandGetMetadata(&metadata);
+                UAVObjSetAccess(&metadata, ACCESS_READWRITE);
+                ManualControlCommandSetMetadata(&metadata);
+            }
+        }
 
-				if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+        if (!ManualControlCommandReadOnly()) {
-					cmd.Channel[n] = PIOS_RCVR_INVALID;
-				} else {
-					cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
-									settings.ChannelNumber[n]);
-				}
-				
-				// If a channel has timed out this is not valid data and we shouldn't update anything
-				// until we decide to go to failsafe
-				if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
-					valid_input_detected = false;
-				else
-					scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n],	settings.ChannelMin[n], settings.ChannelNeutral[n]);
-			}
 
-			// Check settings, if error raise alarm
+            bool valid_input_detected = true;
-			if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-				// Check all channel mappings are valid
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
-				// Check the driver exists
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
-				// Check the FlightModeNumber is valid
-				settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
-				// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
-				((settings.FlightModeNumber > 1) && (
-					settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-					cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
-					cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
 
-				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+            // Read channel values in us
-				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
+            for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
-				ManualControlCommandSet(&cmd);
+                extern uint32_t pios_rcvr_group_map[];
 
-				// Need to do this here since we don't process armed status.  Since this shouldn't happen in flight (changed config) 
+                if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-				// immediately disarm
+                    cmd.Channel[n] = PIOS_RCVR_INVALID;
-				setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+                } else {
+                    cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]);
+                }
 
-				continue;
+                // If a channel has timed out this is not valid data and we shouldn't update anything
-			}
+                // until we decide to go to failsafe
+                if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT)
+                    valid_input_detected = false;
+                else
+                    scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
+            }
 
-			// decide if we have valid manual input or not
+            // Check settings, if error raise alarm
-			valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) &&
+            if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) &&
+                    || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) &&
+                    || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
+                    || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+                    ||
+                    // Check all channel mappings are valid
+                    cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID
+                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID
+                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID
+                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID
+                    ||
+                    // Check the driver exists
+                    cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER
+                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER
+                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER
+                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
+                    // Check the FlightModeNumber is valid
+                    settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
+                    // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
+                    ((settings.FlightModeNumber > 1)
+                            && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
+                                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID
+                                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
 
-			// Implement hysteresis loop on connection status
+                AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-			if (valid_input_detected && (++connected_count > 10)) {
+                cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
-				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
+                ManualControlCommandSet(&cmd);
-				connected_count = 0;
-				disconnected_count = 0;
-			} else if (!valid_input_detected && (++disconnected_count > 10)) {
-				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
-				connected_count = 0;
-				disconnected_count = 0;
-			}
 
-			if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
+                // Need to do this here since we don't process armed status.  Since this shouldn't happen in flight (changed config)
-				cmd.Throttle = -1;	// Shut down engine with no control
+                // immediately disarm
-				cmd.Roll = 0;
+                setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-				cmd.Yaw = 0;
-				cmd.Pitch = 0;
-				cmd.Collective = 0;
-				//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
-				// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
-				// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
-				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				
-				AccessoryDesiredData accessory;
-				// Set Accessory 0
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != 
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					accessory.AccessoryVal = 0;
-					if(AccessoryDesiredInstSet(0, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
-				// Set Accessory 1
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != 
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					accessory.AccessoryVal = 0;
-					if(AccessoryDesiredInstSet(1, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
-				// Set Accessory 2
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != 
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					accessory.AccessoryVal = 0;
-					if(AccessoryDesiredInstSet(2, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
 
-			} else if (valid_input_detected) {
+                continue;
-				AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
+            }
 
-				// Scale channels to -1 -> +1 range
+            // decide if we have valid manual input or not
-				cmd.Roll           = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
+            valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
-				cmd.Pitch          = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
+                    settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
-				cmd.Yaw            = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
+                    && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
-				cmd.Throttle       = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
+                            settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
-				flightMode         = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
+                    && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
+                            settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
+                    && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
+                            settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
 
-				// Apply deadband for Roll/Pitch/Yaw stick inputs
+            // Implement hysteresis loop on connection status
-				if (settings.Deadband > 0.0f) {
+            if (valid_input_detected && (++connected_count > 10)) {
-					applyDeadband(&cmd.Roll, settings.Deadband);
+                cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
-					applyDeadband(&cmd.Pitch, settings.Deadband);
+                connected_count = 0;
-					applyDeadband(&cmd.Yaw, settings.Deadband);
+                disconnected_count = 0;
-				}
+            } else if (!valid_input_detected && (++disconnected_count > 10)) {
+                cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
+                connected_count = 0;
+                disconnected_count = 0;
+            }
+
+            if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
+                cmd.Throttle = -1;	// Shut down engine with no control
+                cmd.Roll = 0;
+                cmd.Yaw = 0;
+                cmd.Pitch = 0;
+                cmd.Collective = 0;
+                //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
+                // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
+                // or leave throttle at IDLE speed or above when going into AUTO-failsafe.
+                AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+
+                AccessoryDesiredData accessory;
+                // Set Accessory 0
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                    accessory.AccessoryVal = 0;
+                    if (AccessoryDesiredInstSet(0, &accessory) != 0)
+                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+                }
+                // Set Accessory 1
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                    accessory.AccessoryVal = 0;
+                    if (AccessoryDesiredInstSet(1, &accessory) != 0)
+                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+                }
+                // Set Accessory 2
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                    accessory.AccessoryVal = 0;
+                    if (AccessoryDesiredInstSet(2, &accessory) != 0)
+                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+                }
+
+            } else if (valid_input_detected) {
+                AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
+
+                // Scale channels to -1 -> +1 range
+                cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
+                cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
+                cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
+                cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
+                flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
+
+                // Apply deadband for Roll/Pitch/Yaw stick inputs
+                if (settings.Deadband > 0.0f) {
+                    applyDeadband(&cmd.Roll, settings.Deadband);
+                    applyDeadband(&cmd.Pitch, settings.Deadband);
+                    applyDeadband(&cmd.Yaw, settings.Deadband);
+                }
 #ifdef USE_INPUT_LPF
-				// Apply Low Pass Filter to input channels, time delta between calls in ms
+                // Apply Low Pass Filter to input channels, time delta between calls in ms
-				portTickType thisSysTime = xTaskGetTickCount();
+                portTickType thisSysTime = xTaskGetTickCount();
-				float dT = (thisSysTime > lastSysTimeLPF) ?
+                float dT = (thisSysTime > lastSysTimeLPF) ?
-						(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
+                (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
-						(float)UPDATE_PERIOD_MS;
+                (float)UPDATE_PERIOD_MS;
-				lastSysTimeLPF = thisSysTime;
+                lastSysTimeLPF = thisSysTime;
 
-				applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
+                applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
-				applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
+                applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
-				applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
+                applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
 #endif // USE_INPUT_LPF
-				if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID &&
+                if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID
-				   cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER &&
+                        && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER
-				   cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT)
+                        && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT)
-					cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
+                    cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
-				   
-				AccessoryDesiredData accessory;
-				// Set Accessory 0
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != 
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
-#ifdef USE_INPUT_LPF
-					applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
-#endif
-					if(AccessoryDesiredInstSet(0, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
-				// Set Accessory 1
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != 
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
-#ifdef USE_INPUT_LPF
-					applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
-#endif
-					if(AccessoryDesiredInstSet(1, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
-				// Set Accessory 2
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != 
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
-#ifdef USE_INPUT_LPF
-					applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
-#endif
-					if(AccessoryDesiredInstSet(2, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
 
-				processFlightMode(&settings, flightMode);
+                AccessoryDesiredData accessory;
+                // Set Accessory 0
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                    accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
+#ifdef USE_INPUT_LPF
+                    applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
+#endif
+                    if (AccessoryDesiredInstSet(0, &accessory) != 0)
+                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+                }
+                // Set Accessory 1
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                    accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
+#ifdef USE_INPUT_LPF
+                    applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
+#endif
+                    if (AccessoryDesiredInstSet(1, &accessory) != 0)
+                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+                }
+                // Set Accessory 2
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                    accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
+#ifdef USE_INPUT_LPF
+                    applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
+#endif
+                    if (AccessoryDesiredInstSet(2, &accessory) != 0)
+                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+                }
 
-			}
+                processFlightMode(&settings, flightMode);
 
-			// Process arming outside conditional so system will disarm when disconnected
+            }
-			processArm(&cmd, &settings);
+
-			
+            // Process arming outside conditional so system will disarm when disconnected
-			// Update cmd object
+            processArm(&cmd, &settings);
-			ManualControlCommandSet(&cmd);
+
+            // Update cmd object
+            ManualControlCommandSet(&cmd);
 #if defined(PIOS_INCLUDE_USB_RCTX)
-			if (pios_usb_rctx_id) {
+            if (pios_usb_rctx_id) {
-				PIOS_USB_RCTX_Update(pios_usb_rctx_id,
+                PIOS_USB_RCTX_Update(pios_usb_rctx_id,
-						cmd.Channel,
+                        cmd.Channel,
-						settings.ChannelMin,
+                        settings.ChannelMin,
-						settings.ChannelMax,
+                        settings.ChannelMax,
-						NELEMENTS(cmd.Channel));
+                        NELEMENTS(cmd.Channel));
-			}
+            }
 #endif	/* PIOS_INCLUDE_USB_RCTX */
 
-		} else {
+        } else {
-			ManualControlCommandGet(&cmd);	/* Under GCS control */
+            ManualControlCommandGet(&cmd); /* Under GCS control */
-		}
+        }
 
-		FlightStatusGet(&flightStatus);
+        FlightStatusGet(&flightStatus);
 
-		// Depending on the mode update the Stabilization or Actuator objects
+        // Depending on the mode update the Stabilization or Actuator objects
-		static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
+        static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
-		switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
+        switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
-			case FLIGHTMODE_UNDEFINED:
+            case FLIGHTMODE_UNDEFINED:
-				// This reflects a bug in the code architecture!
+                // This reflects a bug in the code architecture!
-				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+                AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-				break;
+                break;
-			case FLIGHTMODE_MANUAL:
+            case FLIGHTMODE_MANUAL:
-				updateActuatorDesired(&cmd);
+                updateActuatorDesired(&cmd);
-				break;
+                break;
-			case FLIGHTMODE_STABILIZED:
+            case FLIGHTMODE_STABILIZED:
-				updateStabilizationDesired(&cmd, &settings);
+                updateStabilizationDesired(&cmd, &settings);
-				break;
+                break;
-			case FLIGHTMODE_TUNING:
+            case FLIGHTMODE_TUNING:
-				// Tuning takes settings directly from manualcontrolcommand.  No need to
+                // Tuning takes settings directly from manualcontrolcommand.  No need to
-				// call anything else.  This just avoids errors.
+                // call anything else.  This just avoids errors.
-				break;
+                break;
-			case FLIGHTMODE_GUIDANCE:
+            case FLIGHTMODE_GUIDANCE:
-				switch(flightStatus.FlightMode) {
+                switch (flightStatus.FlightMode) {
-					case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
+                    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
-						altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
+                        altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
-						break;
+                        break;
-					case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+                    case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-						updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
+                    case FLIGHTSTATUS_FLIGHTMODE_POI:
-						break;
+                        updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
-					case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+                        break;
-						updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
+                    case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
-						break;
+                        updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
-					default:
+                        break;
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+                    case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
-				}
+                        // No need to call anything.  This just avoids errors.
-				break;
+                        break;
-		}
+                    case FLIGHTSTATUS_FLIGHTMODE_LAND:
-		lastFlightMode = flightStatus.FlightMode;
+                        updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
-	}
+                        break;
+                    default:
+                        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+                }
+                break;
+        }
+        lastFlightMode = flightStatus.FlightMode;
+    }
 }
 
 static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
 {
-	ReceiverActivityData data;
+    ReceiverActivityData data;
-	bool updated = false;
+    bool updated = false;
 
-	/* Clear all channel activity flags */
+    /* Clear all channel activity flags */
-	ReceiverActivityGet(&data);
+    ReceiverActivityGet(&data);
-	if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE &&
+    if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
-		data.ActiveChannel != 255) {
+        data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
-		data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
+        data.ActiveChannel = 255;
-		data.ActiveChannel = 255;
+        updated = true;
-		updated = true;
+    }
-	}
+    if (updated) {
-	if (updated) {
+        ReceiverActivitySet(&data);
-		ReceiverActivitySet(&data);
+    }
-	}
 
-	/* Reset the FSM state */
+    /* Reset the FSM state */
-	fsm->group        = 0;
+    fsm->group = 0;
-	fsm->sample_count = 0;
+    fsm->sample_count = 0;
 }
 
-static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) {
+static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
-	for (uint8_t channel = 1; channel <= max_channels; channel++) {
+{
-		// Subtract 1 because channels are 1 indexed
+    for (uint8_t channel = 1; channel <= max_channels; channel++) {
-		samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
+        // Subtract 1 because channels are 1 indexed
-	}
+        samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
+    }
 }
 
 static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm)
 {
-	bool activity_updated = false;
+    bool activity_updated = false;
 
-	/* Compare the current value to the previous sampled value */
+    /* Compare the current value to the previous sampled value */
-	for (uint8_t channel = 1;
+    for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
-	     channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP;
+        uint16_t delta;
-	     channel++) {
+        uint16_t prev = fsm->prev[channel - 1];   // Subtract 1 because channels are 1 indexed
-		uint16_t delta;
+        uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
-		uint16_t prev = fsm->prev[channel - 1];   // Subtract 1 because channels are 1 indexed
+        if (curr > prev) {
-		uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); 
+            delta = curr - prev;
-		if (curr > prev) {
+        } else {
-			delta = curr - prev;
+            delta = prev - curr;
-		} else {
+        }
-			delta = prev - curr;
-		}
 
-		if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
+        if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
-			/* Mark this channel as active */
+            /* Mark this channel as active */
-			ReceiverActivityActiveGroupOptions group;
+            ReceiverActivityActiveGroupOptions group;
 
-			/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
+            /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
-			switch (fsm->group) {
+            switch (fsm->group) {
-			case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: 
+                case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
-				group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
+                    group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
-				break;
+                    break;
-			case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
+                case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
-				group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
+                    group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
-				break;
+                    break;
-			case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
+                case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
-				group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
+                    group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
-				break;
+                    break;
-			case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
+                case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
-				group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
+                    group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
-				break;
+                    break;
-			case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
+                case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
-				group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
+                    group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
-				break;
+                    break;
-			case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
+                case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
-				group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
+                    group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
-				break;
+                    break;
-			case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
+                case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
-				group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
+                    group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
-				break;
+                    break;
-			default:
+                default:
-				PIOS_Assert(0);
+                    PIOS_Assert(0);
-				break;
+                    break;
-			}
+            }
 
-			ReceiverActivityActiveGroupSet((uint8_t*)&group);
+            ReceiverActivityActiveGroupSet((uint8_t*) &group);
-			ReceiverActivityActiveChannelSet(&channel);
+            ReceiverActivityActiveChannelSet(&channel);
-			activity_updated = true;
+            activity_updated = true;
-		}
+        }
-	}
+    }
-	return (activity_updated);
+    return (activity_updated);
 }
 
 static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
 {
-	bool activity_updated = false;
+    bool activity_updated = false;
 
-	if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+    if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-		/* We're out of range, reset things */
+        /* We're out of range, reset things */
-		resetRcvrActivity(fsm);
+        resetRcvrActivity(fsm);
-	}
+    }
 
-	extern uint32_t pios_rcvr_group_map[];
+    extern uint32_t pios_rcvr_group_map[];
-	if (!pios_rcvr_group_map[fsm->group]) {
+    if (!pios_rcvr_group_map[fsm->group]) {
-		/* Unbound group, skip it */
+        /* Unbound group, skip it */
-		goto group_completed;
+        goto group_completed;
-	}
+    }
 
-	if (fsm->sample_count == 0) {
+    if (fsm->sample_count == 0) {
-		/* Take a sample of each channel in this group */
+        /* Take a sample of each channel in this group */
-		updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
+        updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
-					fsm->prev,
+        fsm->sample_count++;
-					NELEMENTS(fsm->prev));
+        return (false);
-		fsm->sample_count++;
+    }
-		return (false);
-	}
 
-	/* Compare with previous sample */
+    /* Compare with previous sample */
-	activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
+    activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
 
-group_completed:
+    group_completed:
-	/* Reset the sample counter */
+    /* Reset the sample counter */
-	fsm->sample_count = 0;
+    fsm->sample_count = 0;
 
-	/* Find the next active group, but limit search so we can't loop forever here */
+    /* Find the next active group, but limit search so we can't loop forever here */
-	for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
+    for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
-		/* Move to the next group */
+        /* Move to the next group */
-		fsm->group++;
+        fsm->group++;
-		if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+        if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-			/* Wrap back to the first group */
+            /* Wrap back to the first group */
-			fsm->group = 0;
+            fsm->group = 0;
-		}
+        }
-		if (pios_rcvr_group_map[fsm->group]) {
+        if (pios_rcvr_group_map[fsm->group]) {
-			/* 
+            /*
-			 * Found an active group, take a sample here to avoid an
+             * Found an active group, take a sample here to avoid an
-			 * extra 20ms delay in the main thread so we can speed up
+             * extra 20ms delay in the main thread so we can speed up
-			 * this algorithm.
+             * this algorithm.
-			 */
+             */
-			updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
+            updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
-						fsm->prev,
+            fsm->sample_count++;
-						NELEMENTS(fsm->prev));
+            break;
-			fsm->sample_count++;
+        }
-			break;
+    }
-		}
-	}
 
-	return (activity_updated);
+    return (activity_updated);
 }
 
 static void updateActuatorDesired(ManualControlCommandData * cmd)
 {
-	ActuatorDesiredData actuator;
+    ActuatorDesiredData actuator;
-	ActuatorDesiredGet(&actuator);
+    ActuatorDesiredGet(&actuator);
-	actuator.Roll = cmd->Roll;
+    actuator.Roll = cmd->Roll;
-	actuator.Pitch = cmd->Pitch;
+    actuator.Pitch = cmd->Pitch;
-	actuator.Yaw = cmd->Yaw;
+    actuator.Yaw = cmd->Yaw;
-	actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
+    actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
-	ActuatorDesiredSet(&actuator);
+    ActuatorDesiredSet(&actuator);
 }
 
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
 {
-	StabilizationDesiredData stabilization;
+    StabilizationDesiredData stabilization;
-	StabilizationDesiredGet(&stabilization);
+    StabilizationDesiredGet(&stabilization);
 
-	StabilizationSettingsData stabSettings;
+    StabilizationSettingsData stabSettings;
-	StabilizationSettingsGet(&stabSettings);
+    StabilizationSettingsGet(&stabSettings);
 
-	uint8_t * stab_settings;
+    uint8_t * stab_settings;
-	FlightStatusData flightStatus;
+    FlightStatusData flightStatus;
-	FlightStatusGet(&flightStatus);
+    FlightStatusGet(&flightStatus);
-	switch(flightStatus.FlightMode) {
+    switch (flightStatus.FlightMode) {
-		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
-			stab_settings = settings->Stabilization1Settings;
+            stab_settings = settings->Stabilization1Settings;
-			break;
+            break;
-		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
-			stab_settings = settings->Stabilization2Settings;
+            stab_settings = settings->Stabilization2Settings;
-			break;
+            break;
-		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
+        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-			stab_settings = settings->Stabilization3Settings;
+            stab_settings = settings->Stabilization3Settings;
-			break;
+            break;
-		default:
+        default:
-			// Major error, this should not occur because only enter this block when one of these is true
+            // Major error, this should not occur because only enter this block when one of these is true
-			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+            AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-			return;
+            return;
-	}
+    }
 
-	// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
+    // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
-	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL]  = stab_settings[0];
+    stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
-	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
+    stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
-	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW]   = stab_settings[2];
+    stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
 
-	stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
+    stabilization.Roll =
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+                    cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
-	     0; // this is an invalid mode
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
-					      ;
+                    cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
-	stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+    ;
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+    stabilization.Pitch =
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+                    cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
-	     0; // this is an invalid mode
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
+                    cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
+                    cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
 
-	stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
+    stabilization.Yaw =
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+                    cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
-	     0; // this is an invalid mode
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
 
-	stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
+    stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
-	StabilizationDesiredSet(&stabilization);
+    StabilizationDesiredSet(&stabilization);
 }
 
 #if defined(REVOLUTION)
@@ -696,52 +702,92 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
  * @brief Update the position desired to current location when
  * enabled and allow the waypoint to be moved by transmitter
  */
-static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
+static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed,bool home)
 {
-	if (home && changed) {
+    /*
-		// Simple Return To Base mode - keep altitude the same, fly to home position
+    static portTickType lastSysTime;
-		PositionActualData positionActual;
+    portTickType thisSysTime = xTaskGetTickCount();
-		PositionActualGet(&positionActual);
+    dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
-		
+    lastSysTime = thisSysTime;
-		PathDesiredData pathDesired;
+    */
-		PathDesiredGet(&pathDesired);
+
-		pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
+    if (home && changed) {
-		pathDesired.Start[PATHDESIRED_START_EAST] = 0;
+        // Simple Return To Base mode - keep altitude the same, fly to home position
-		pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
+        PositionActualData positionActual;
-		pathDesired.End[PATHDESIRED_END_NORTH] = 0;
+        PositionActualGet(&positionActual);
-		pathDesired.End[PATHDESIRED_END_EAST] = 0;
+
-		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
+        PathDesiredData pathDesired;
-		pathDesired.StartingVelocity=1;
+        PathDesiredGet(&pathDesired);
-		pathDesired.EndingVelocity=0;
+        pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
-		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+        pathDesired.Start[PATHDESIRED_START_EAST] = 0;
-		PathDesiredSet(&pathDesired);
+        pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
-	} else if(changed) {
+        pathDesired.End[PATHDESIRED_END_NORTH] = 0;
-		// After not being in this mode for a while init at current height
+        pathDesired.End[PATHDESIRED_END_EAST] = 0;
-		PositionActualData positionActual;
+        pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
-		PositionActualGet(&positionActual);
+        pathDesired.StartingVelocity=1;
-		
+        pathDesired.EndingVelocity=0;
-		PathDesiredData pathDesired;
+        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-		PathDesiredGet(&pathDesired);
+        PathDesiredSet(&pathDesired);
-		pathDesired.Start[PATHDESIRED_END_NORTH] = positionActual.North;
+    } else if(changed) {
-		pathDesired.Start[PATHDESIRED_END_EAST] = positionActual.East;
+        // After not being in this mode for a while init at current height
-		pathDesired.Start[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
+        PositionActualData positionActual;
-		pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
+        PositionActualGet(&positionActual);
-		pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
+
-		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
+        PathDesiredData pathDesired;
-		pathDesired.StartingVelocity=1;
+        PathDesiredGet(&pathDesired);
-		pathDesired.EndingVelocity=0;
+        pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
-		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+        pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
-		PathDesiredSet(&pathDesired);
+        pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
-	/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
+        pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
-	} else {
+        pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
-		PathDesiredData pathDesired;
+        pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
-		PathDesiredGet(&pathDesired);
+        pathDesired.StartingVelocity=1;
-		pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
+        pathDesired.EndingVelocity=0;
-		pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
+        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+        PathDesiredSet(&pathDesired);
-		PathDesiredSet(&pathDesired);
+        /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
+         } else {
+         PathDesiredData pathDesired;
+         PathDesiredGet(&pathDesired);
+         pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
+         pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
+         pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+         PathDesiredSet(&pathDesired);
+         */
+    }
+}
+
+static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd, bool changed)
+{
+    /*
+    static portTickType lastSysTime;
+    portTickType thisSysTime;
+    float dT;
+
+	thisSysTime = xTaskGetTickCount();
+	dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
+	lastSysTime = thisSysTime;
 	*/
-	}
+
+    PositionActualData positionActual;
+    PositionActualGet(&positionActual);
+
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+    if(changed) {
+        // After not being in this mode for a while init at current height
+        pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
+        pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
+        pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
+        pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
+        pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
+        pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
+        pathDesired.StartingVelocity=1;
+        pathDesired.EndingVelocity=0;
+        pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+    }
+    pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5;
+    PathDesiredSet(&pathDesired);
 }
 
 /**
@@ -751,55 +797,66 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
  */
 static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
 {
-	const float DEADBAND_HIGH = 0.55f;
+    const float DEADBAND_HIGH = 0.55f;
-	const float DEADBAND_LOW = 0.45f;
+    const float DEADBAND_LOW = 0.45f;
-	
-	static portTickType lastSysTime;
-	static bool zeroed = false;
-	portTickType thisSysTime;
-	float dT;
-	AltitudeHoldDesiredData altitudeHoldDesired;
-	AltitudeHoldDesiredGet(&altitudeHoldDesired);
 
-	StabilizationSettingsData stabSettings;
+    static portTickType lastSysTime;
-	StabilizationSettingsGet(&stabSettings);
+    static bool zeroed = false;
+    portTickType thisSysTime;
+    float dT;
+    AltitudeHoldDesiredData altitudeHoldDesired;
+    AltitudeHoldDesiredGet(&altitudeHoldDesired);
+
+    StabilizationSettingsData stabSettings;
+    StabilizationSettingsGet(&stabSettings);
 
 	thisSysTime = xTaskGetTickCount();
-	dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f);
+	dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
 	lastSysTime = thisSysTime;
 
-	altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
+    altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
-	altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
+    altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
-	altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
+    altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
-	
+
-	float currentDown;
+    float currentDown;
-	PositionActualDownGet(&currentDown);
+    PositionActualDownGet(&currentDown);
-	if(changed) {
+    if(changed) {
-		// After not being in this mode for a while init at current height
+        // After not being in this mode for a while init at current height
-		altitudeHoldDesired.Altitude = 0;
+        altitudeHoldDesired.Altitude = 0;
-		zeroed = false;
+        zeroed = false;
-	} else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
+    } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
-		altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
+        altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
-	else if (cmd->Throttle < DEADBAND_LOW && zeroed)
+    } else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
-		altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
+        altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
-	else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH)  // Require the stick to enter the dead band before they can move height
+    } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
-		zeroed = true;
+        // Require the stick to enter the dead band before they can move height
-	
+        zeroed = true;
-	AltitudeHoldDesiredSet(&altitudeHoldDesired);
+    }
+
+    AltitudeHoldDesiredSet(&altitudeHoldDesired);
 }
 #else
 
 // TODO: These functions should never be accessible on CC.  Any configuration that
 // could allow them to be called sholud already throw an error to prevent this happening
 // in flight
-static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home)
+static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * cmd,
+								  __attribute__((unused)) bool changed,
+								  __attribute__((unused)) bool home)
 {
-	AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
+    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
 }
 
-static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
+static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * cmd,
+								  __attribute__((unused)) bool changed)
 {
-	AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
+    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
+}
+
+static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData * cmd,
+									__attribute__((unused)) bool changed)
+{
+    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
 }
 #endif
 /**
@@ -807,38 +864,35 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
  */
 static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
 {
-	float valueScaled;
+    float valueScaled;
 
-	// Scale
+    // Scale
-	if ((max > min && value >= neutral) || (min > max && value <= neutral))
+    if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
-	{
+        if (max != neutral) {
-		if (max != neutral)
+            valueScaled = (float) (value - neutral) / (float) (max - neutral);
-			valueScaled = (float)(value - neutral) / (float)(max - neutral);
+        } else {
-		else
+            valueScaled = 0;
-			valueScaled = 0;
+        }
-	}
+    } else {
-	else
+        if (min != neutral) {
-	{
+            valueScaled = (float) (value - neutral) / (float) (neutral - min);
-		if (min != neutral)
+        } else {
-			valueScaled = (float)(value - neutral) / (float)(neutral - min);
+            valueScaled = 0;
-		else
+        }
-			valueScaled = 0;
+    }
-	}
 
-	// Bound
+    // Bound
-	if (valueScaled >  1.0f) {
+    if (valueScaled > 1.0f) {
-		valueScaled =  1.0f;
+        valueScaled = 1.0f;
-	} else if (valueScaled < -1.0f) {
+    } else if (valueScaled < -1.0f) {
-		valueScaled = -1.0f;
+        valueScaled = -1.0f;
-	}
+    }
 
-	return valueScaled;
+    return valueScaled;
 }
 
 static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
-	if(end_time > start_time)
+	return (end_time - start_time) * portTICK_RATE_MS;
-		return (end_time - start_time) * portTICK_RATE_MS;
-	return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
 }
 
 /**
@@ -847,24 +901,21 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
  */
 static bool okToArm(void)
 {
-	// read alarms
+    // read alarms
-	SystemAlarmsData alarms;
+    SystemAlarmsData alarms;
-	SystemAlarmsGet(&alarms);
+    SystemAlarmsGet(&alarms);
 
+    // Check each alarm
+    for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
+        if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) {	// found an alarm thats set
+            if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
+                continue;
+			}
+            return false;
+        }
+    }
 
-	// Check each alarm
+    return true;
-	for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
-	{
-		if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
-		{	// found an alarm thats set
-			if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
-				continue;
-
-			return false;
-		}
-	}
-
-	return true;
 }
 /**
  * @brief Determine if the aircraft is forced to disarm by an explicit alarm
@@ -872,28 +923,29 @@ static bool okToArm(void)
  */
 static bool forcedDisArm(void)
 {
-	// read alarms
+    // read alarms
-	SystemAlarmsData alarms;
+    SystemAlarmsData alarms;
-	SystemAlarmsGet(&alarms);
+    SystemAlarmsGet(&alarms);
 
-	if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
+    if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
-		return true;
+        return true;
-	}
+    }
-	return false;
+    return false;
 }
 
 /**
  * @brief Update the flightStatus object only if value changed.  Reduces callbacks
  * @param[in] val The new value
  */
-static void setArmedIfChanged(uint8_t val) {
+static void setArmedIfChanged(uint8_t val)
-	FlightStatusData flightStatus;
+{
-	FlightStatusGet(&flightStatus);
+    FlightStatusData flightStatus;
+    FlightStatusGet(&flightStatus);
 
-	if(flightStatus.Armed != val) {
+    if (flightStatus.Armed != val) {
-		flightStatus.Armed = val;
+        flightStatus.Armed = val;
-		FlightStatusSet(&flightStatus);
+        FlightStatusSet(&flightStatus);
-	}
+    }
 }
 
 /**
@@ -904,116 +956,121 @@ static void setArmedIfChanged(uint8_t val) {
 static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
 {
 
-	bool lowThrottle = cmd->Throttle <= 0;
+    bool lowThrottle = cmd->Throttle <= 0;
 
-	if (forcedDisArm()) {
+    if (forcedDisArm()) {
-		// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
+        // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
-		setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-		return;
+        return;
-	}
+    }
 
-	if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
+    if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
-		// In this configuration we always disarm
+        // In this configuration we always disarm
-		setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-	} else {
+    } else {
-		// Not really needed since this function not called when disconnected
+        // Not really needed since this function not called when disconnected
-		if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
+        if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
-			lowThrottle = true;
+            lowThrottle = true;
 
-		// The throttle is not low, in case we where arming or disarming, abort
+        // The throttle is not low, in case we where arming or disarming, abort
-		if (!lowThrottle) {
+        if (!lowThrottle) {
-			switch(armState) {
+            switch (armState) {
-				case ARM_STATE_DISARMING_MANUAL:
+                case ARM_STATE_DISARMING_MANUAL:
-				case ARM_STATE_DISARMING_TIMEOUT:
+                case ARM_STATE_DISARMING_TIMEOUT:
-					armState = ARM_STATE_ARMED;
+                    armState = ARM_STATE_ARMED;
-					break;
+                    break;
-				case ARM_STATE_ARMING_MANUAL:
+                case ARM_STATE_ARMING_MANUAL:
-					armState = ARM_STATE_DISARMED;
+                    armState = ARM_STATE_DISARMED;
-					break;
+                    break;
-				default:
+                default:
-					// Nothing needs to be done in the other states
+                    // Nothing needs to be done in the other states
-					break;
+                    break;
-			}
+            }
-			return;
+            return;
-		}
+        }
 
-		// The rest of these cases throttle is low
+        // The rest of these cases throttle is low
-		if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
+        if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
-			// In this configuration, we go into armed state as soon as the throttle is low, never disarm
+            // In this configuration, we go into armed state as soon as the throttle is low, never disarm
-			setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
+            setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
-			return;
+            return;
-		}
+        }
 
+        // When the configuration is not "Always armed" and no "Always disarmed",
+        // the state will not be changed when the throttle is not low
+        static portTickType armedDisarmStart;
+        float armingInputLevel = 0;
 
-		// When the configuration is not "Always armed" and no "Always disarmed",
+        // Calc channel see assumptions7
-		// the state will not be changed when the throttle is not low
+        int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
-		static portTickType armedDisarmStart;
+        switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
-		float armingInputLevel = 0;
+            case ARMING_CHANNEL_ROLL:
+                armingInputLevel = sign * cmd->Roll;
+                break;
+            case ARMING_CHANNEL_PITCH:
+                armingInputLevel = sign * cmd->Pitch;
+                break;
+            case ARMING_CHANNEL_YAW:
+                armingInputLevel = sign * cmd->Yaw;
+                break;
+        }
 
-		// Calc channel see assumptions7
+        bool manualArm = false;
-		int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
+        bool manualDisarm = false;
-		switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
-			case ARMING_CHANNEL_ROLL:    armingInputLevel = sign * cmd->Roll;    break;
-			case ARMING_CHANNEL_PITCH:   armingInputLevel = sign * cmd->Pitch;   break;
-			case ARMING_CHANNEL_YAW:     armingInputLevel = sign * cmd->Yaw;     break;
-		}
 
-		bool manualArm = false;
+        if (armingInputLevel <= -ARMED_THRESHOLD)
-		bool manualDisarm = false;
+            manualArm = true;
+        else if (armingInputLevel >= +ARMED_THRESHOLD)
+            manualDisarm = true;
 
-		if (armingInputLevel <= -ARMED_THRESHOLD)
+        switch (armState) {
-			manualArm = true;
+            case ARM_STATE_DISARMED:
-		else if (armingInputLevel >= +ARMED_THRESHOLD)
+                setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-			manualDisarm = true;
 
-		switch(armState) {
+                // only allow arming if it's OK too
-			case ARM_STATE_DISARMED:
+                if (manualArm && okToArm()) {
-				setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+                    armedDisarmStart = lastSysTime;
+                    armState = ARM_STATE_ARMING_MANUAL;
+                }
+                break;
 
-				// only allow arming if it's OK too
+            case ARM_STATE_ARMING_MANUAL:
-				if (manualArm && okToArm()) {
+                setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
-					armedDisarmStart = lastSysTime;
-					armState = ARM_STATE_ARMING_MANUAL;
-				}
-				break;
 
-			case ARM_STATE_ARMING_MANUAL:
+                if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
-				setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
+                    armState = ARM_STATE_ARMED;
+                else if (!manualArm)
+                    armState = ARM_STATE_DISARMED;
+                break;
 
-				if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
+            case ARM_STATE_ARMED:
-					armState = ARM_STATE_ARMED;
+                // When we get here, the throttle is low,
-				else if (!manualArm)
+                // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
-					armState = ARM_STATE_DISARMED;
+                armedDisarmStart = lastSysTime;
-				break;
+                armState = ARM_STATE_DISARMING_TIMEOUT;
+                setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
+                break;
 
-			case ARM_STATE_ARMED:
+            case ARM_STATE_DISARMING_TIMEOUT:
-				// When we get here, the throttle is low,
+                // We get here when armed while throttle low, even when the arming timeout is not enabled
-				// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
+                if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
-				armedDisarmStart = lastSysTime;
+                    armState = ARM_STATE_DISARMED;
-				armState = ARM_STATE_DISARMING_TIMEOUT;
-				setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
-				break;
 
-			case ARM_STATE_DISARMING_TIMEOUT:
+                // Switch to disarming due to manual control when needed
-				// We get here when armed while throttle low, even when the arming timeout is not enabled
+                if (manualDisarm) {
-				if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
+                    armedDisarmStart = lastSysTime;
-					armState = ARM_STATE_DISARMED;
+                    armState = ARM_STATE_DISARMING_MANUAL;
+                }
+                break;
 
-				// Switch to disarming due to manual control when needed
+            case ARM_STATE_DISARMING_MANUAL:
-				if (manualDisarm) {
+                if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
-					armedDisarmStart = lastSysTime;
+                    armState = ARM_STATE_DISARMED;
-					armState = ARM_STATE_DISARMING_MANUAL;
+                else if (!manualDisarm)
-				}
+                    armState = ARM_STATE_ARMED;
-				break;
+                break;
-
+        }	// End Switch
-			case ARM_STATE_DISARMING_MANUAL:
+    }
-				if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
-					armState = ARM_STATE_DISARMED;
-				else if (!manualDisarm)
-					armState = ARM_STATE_ARMED;
-				break;
-		}	// End Switch
-	}
 }
 
 /**
@@ -1024,20 +1081,20 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
  */
 static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
 {
-	FlightStatusData flightStatus;
+    FlightStatusData flightStatus;
-	FlightStatusGet(&flightStatus);
+    FlightStatusGet(&flightStatus);
 
-	// Convert flightMode value into the switch position in the range [0..N-1]
+    // Convert flightMode value into the switch position in the range [0..N-1]
-	uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
+    uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
-	if (pos >= settings->FlightModeNumber)
+    if (pos >= settings->FlightModeNumber)
-		pos = settings->FlightModeNumber - 1;
+        pos = settings->FlightModeNumber - 1;
 
-	uint8_t newMode = settings->FlightModePosition[pos];
+    uint8_t newMode = settings->FlightModePosition[pos];
 
-	if (flightStatus.FlightMode != newMode) {
+    if (flightStatus.FlightMode != newMode) {
-		flightStatus.FlightMode = newMode;
+        flightStatus.FlightMode = newMode;
-		FlightStatusSet(&flightStatus);
+        FlightStatusSet(&flightStatus);
-	}
+    }
 }
 
 /**
@@ -1046,13 +1103,12 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
  */
 bool validInputRange(int16_t min, int16_t max, uint16_t value)
 {
-	if (min > max)
+    if (min > max) {
-	{
+        int16_t tmp = min;
-		int16_t tmp = min;
+        min = max;
-		min = max;
+        max = tmp;
-		max = tmp;
+    }
-	}
+    return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
-	return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
 }
 
 /**
@@ -1060,13 +1116,12 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
  */
 static void applyDeadband(float *value, float deadband)
 {
-	if (fabs(*value) < deadband)
+    if (fabsf(*value) < deadband)
-		*value = 0.0f;
+        *value = 0.0f;
-	else
+    else if (*value > 0.0f)
-		if (*value > 0.0f)
+        *value -= deadband;
-			*value -= deadband;
+    else
-		else
+        *value += deadband;
-			*value += deadband;
 }
 
 #ifdef USE_INPUT_LPF
@@ -1075,25 +1130,22 @@ static void applyDeadband(float *value, float deadband)
  */
 static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
 {
-	if (settings->ResponseTime[channel]) {
+    if (settings->ResponseTime[channel]) {
-		float rt = (float)settings->ResponseTime[channel];
+        float rt = (float)settings->ResponseTime[channel];
-		inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
+        inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
-		*value = inputFiltered[channel];
+        *value = inputFiltered[channel];
-	}
+    }
 }
 #endif // USE_INPUT_LPF
-
 /**
  * Called whenever a critical configuration component changes
  */
-static void configurationUpdatedCb(UAVObjEvent * ev)
+static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-	configuration_check();
+    configuration_check();
 }
 
-
-
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

flight/modules/OPLink/oplinkmod.c

@@ -123,7 +123,7 @@ MODULE_INITCALL(OPLinkModInitialize, 0)
 /**
  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
  */
-static void systemTask(void *parameters)
+static void systemTask(__attribute__((unused)) void *parameters)
 {
 	portTickType lastSysTime;
 	uint16_t prev_tx_count = 0;
@@ -227,7 +227,8 @@ void vApplicationIdleHook(void)
  * Called by the RTOS when a stack overflow is detected.
  */
 #define DEBUG_STACK_OVERFLOW 0
-void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
+void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle * pxTask,
+								   __attribute__((unused)) signed portCHAR * pcTaskName)
 {
 	stackOverflow = true;
 #if DEBUG_STACK_OVERFLOW

flight/modules/Osd/OsdEtStd/OsdEtStd.c

@@ -41,7 +41,7 @@
 //
 #define DEBUG_PORT		PIOS_COM_GPS
 //#define ENABLE_DEBUG_MSG
-//#define USE_DEBUG_PINS
+//#define PIOS_ENABLE_DEBUG_PINS
 //#define DUMP_CONFIG           // Enable this do read and dump the OSD config
 
 //
@@ -78,7 +78,7 @@
 #define OSDMSG_GPS_STAT_FIX		0x2B
 #define OSDMSG_GPS_STAT_HB_FLAG	0x10
 
-#ifdef USE_DEBUG_PINS
+#ifdef PIOS_ENABLE_DEBUG_PINS
 	#define	DEBUG_PIN_RUNNING	0
 	#define	DEBUG_PIN_I2C		1
 	#define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
@@ -206,17 +206,17 @@ static void SetNbSats(uint8_t nb)
 	msg[OSDMSG_NB_SATS] = nb;
 }
 
-static void FlightBatteryStateUpdatedCb(UAVObjEvent * ev)
+static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	newBattData = TRUE;
 }
 
-static void GPSPositionUpdatedCb(UAVObjEvent * ev)
+static void GPSPositionUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	newPosData = TRUE;
 }
 
-static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
+static void BaroAltitudeUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
 	newBaroData = TRUE;
 }

flight/modules/Osd/WavPlayer/inc/wavplayer.h

@@ -1,15 +1,37 @@
-/*
+/**
- * WavPlayer.h
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup WavPlayerModule WavPlayer Module
+ * @brief Process WavPlayer information
+ * @{
  *
- *  Created on: 15.07.2012
+ * @file       wavplayer.h
- *      Author: Samba
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      WavPlayer module
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-
+#ifndef WAVPLAYER_H_
-#ifndef WavPlayer_H_
+#define WAVPLAYER_H_
-#define WavPlayer_H_
 
 #include "openpilot.h"
 
 int32_t WavPlayerInitialize(void);
 
-#endif /* WavPlayer_H_ */
+#endif /* WAVPLAYER_H_ */

flight/modules/Osd/WavPlayer/wavplayer.c

@@ -6,7 +6,7 @@
  * @brief Process WavPlayer information
  * @{
  *
- * @file       WavPlayer.c
+ * @file       wavplayer.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  * @brief      WavPlayer module
  * @see        The GNU Public License (GPL) Version 3
@@ -29,10 +29,8 @@
  */
 
 // ****************
-
 #include "openpilot.h"
 
-
 // ****************
 // Private functions
 
@@ -56,10 +54,10 @@ static uint32_t timeOfLastUpdateMs;
 // ****************
 int32_t WavPlayerStart(void)
 {
-	// Start WavPlayer task
+    // Start WavPlayer task
-	xTaskCreate(WavPlayerTask, (signed char *)"WavPlayer", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle);
+    xTaskCreate(WavPlayerTask, (signed char *) "WavPlayer", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &WavPlayerTaskHandle);
 
-	return 0;
+    return 0;
 }
 /**
  * Initialise the WavPlayer module
@@ -69,54 +67,37 @@ int32_t WavPlayerStart(void)
 int32_t WavPlayerInitialize(void)
 {
 
-	return 0;
+    return 0;
 }
-MODULE_INITCALL(WavPlayerInitialize, WavPlayerStart)
+MODULE_INITCALL( WavPlayerInitialize, WavPlayerStart)
 
 // ****************
 /**
- * Main gps task. It does not return.
+ * Main WavPlayer task. It does not return.
  */
 
-static void WavPlayerTask(void *parameters)
+static void WavPlayerTask(__attribute__((unused)) void *parameters)
 {
-	portTickType lastSysTime;
+    portTickType lastSysTime;
-	// Loop forever
+    // Loop forever
-	lastSysTime = xTaskGetTickCount();	//portTickType xDelay = 100 / portTICK_RATE_MS;
+    lastSysTime = xTaskGetTickCount();
-	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
+    uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
 
-
+    timeOfLastUpdateMs = timeNowMs;
-	timeOfLastUpdateMs = timeNowMs;
+    timeOfLastCommandMs = timeNowMs;
-	timeOfLastCommandMs = timeNowMs;
 #if defined(PIOS_INCLUDE_WAVE)
-	WavePlayer_Start();
+    WavePlayer_Start();
 #endif
-	// Loop forever
+    // Loop forever
-	while (1)
+    while (1) {
-	{
+        vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
-
+        timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-		vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
+    }
-		// Check for GPS timeout
-		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-		/*if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
-		{	// we have not received any valid GPS sentences for a while.
-			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
-
-
-		}
-		else
-		{	// we appear to be receiving GPS sentences OK, we've had an update
-
-
-		}*/
-
-	}
 }
 
-
 // ****************
 
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

flight/modules/Osd/osdgen/inc/osdgen.h

@@ -1,18 +1,41 @@
-/*
+/**
- * osdgen.h
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup OSDgenModule osdgen Module
+ * @brief Process OSD information
+ * @{
  *
- *  Created on: 2.10.2011
+ * @file       osdgen.h
- *      Author: Samba
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      OSD gen module, handles OSD draw. Parts from CL-OSD and SUPEROSD projects
+ * @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 OSDGEN_H_
 #define OSDGEN_H_
 
 #include "openpilot.h"
+#include "pios.h"
 
 int32_t osdgenInitialize(void);
 
-
 // Size of an array (num items.)
 #define SIZEOF_ARRAY(x) (sizeof(x) / sizeof((x)[0]))
 
@@ -78,12 +101,9 @@ int32_t osdgenInitialize(void);
         write_pixel(buff, (cx) + (x), (cy) - (y), mode); \
         write_pixel(buff, (cx) - (x), (cy) - (y), mode);
 
-
-
 // Font flags.
 #define FONT_BOLD               1               // bold text (no outline)
 #define FONT_INVERT             2               // invert: border white, inside black
-
 // Text alignments.
 #define TEXT_VA_TOP     0
 #define TEXT_VA_MIDDLE  1
@@ -95,10 +115,9 @@ int32_t osdgenInitialize(void);
 // Text dimension structures.
 struct FontDimensions
 {
-        int width, height;
+    int width, height;
 };
 
-
 // Max/Min macros.
 #define MAX(a, b)               ((a) > (b) ? (a) : (b))
 #define MIN(a, b)               ((a) < (b) ? (a) : (b))
@@ -110,20 +129,19 @@ struct FontDimensions
 #define APPLY_VDEADBAND(y) ((y)+GRAPHICS_VDEADBAND)
 #define APPLY_HDEADBAND(x) ((x)+GRAPHICS_HDEADBAND)
 
-// Check if coordinates are valid. If not, return.
+// Check if coordinates are valid. If not, return. Assumes unsigned coordinate
-#define CHECK_COORDS(x, y) if(x < 0 || x >= GRAPHICS_WIDTH_REAL || y < 0 || y >= GRAPHICS_HEIGHT_REAL) return;
+#define CHECK_COORDS(x, y) if(x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) return;
-#define CHECK_COORD_X(x) if(x < 0 || x >= GRAPHICS_WIDTH_REAL) return;
+#define CHECK_COORD_X(x) if(x >= GRAPHICS_WIDTH_REAL) return;
-#define CHECK_COORD_Y(y) if(y < 0 || y >= GRAPHICS_HEIGHT_REAL) return;
+#define CHECK_COORD_Y(y) if(y >= GRAPHICS_HEIGHT_REAL) return;
 
-// Clip coordinates out of range.
+// Clip coordinates out of range - assumes unsigned coordinate
-#define CLIP_COORD_X(x) { x = MAX(0, MIN(x, GRAPHICS_WIDTH_REAL)); }
+#define CLIP_COORD_X(x) { x = MIN(x, GRAPHICS_WIDTH_REAL); }
-#define CLIP_COORD_Y(y) { y = MAX(0, MIN(y, GRAPHICS_HEIGHT_REAL)); }
+#define CLIP_COORD_Y(y) { y = MIN(y, GRAPHICS_HEIGHT_REAL); }
 #define CLIP_COORDS(x, y) { CLIP_COORD_X(x); CLIP_COORD_Y(y); }
 
 // Macro to swap two variables using XOR swap.
 #define SWAP(a, b) { a ^= b; b ^= a; a ^= b; }
 
-
 // Line triggering
 #define LAST_LINE 312 //625/2 //PAL
 //#define LAST_LINE 525/2 //NTSC
@@ -141,8 +159,6 @@ struct FontDimensions
 #define DELAY_9_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n")
 #define DELAY_10_NOP() asm("nop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\nnop\r\n")
 
-
-
 uint8_t getCharData(uint16_t charPos);
 void introText();
 
@@ -189,7 +205,4 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
 
 void updateOnceEveryFrame();
 
-
-
-
 #endif /* OSDGEN_H_ */

flight/modules/Osd/osdgen/osdgen.c

@@ -2,7 +2,7 @@
  ******************************************************************************
  * @addtogroup OpenPilotModules OpenPilot Modules
  * @{
- * @addtogroup OSDGENModule osdgen Module
+ * @addtogroup OSDgenModule osdgen Module
  * @brief Process OSD information
  * @{
  *
@@ -42,6 +42,7 @@
 #include "osdsettings.h"
 #include "baroaltitude.h"
 #include "taskinfo.h"
+#include "flightstatus.h"
 
 #include "fonts.h"
 #include "font12x18.h"
@@ -50,29 +51,28 @@
 
 #include "splash.h"
 /*
-static uint16_t angleA=0;
+ static uint16_t angleA=0;
-static int16_t angleB=90;
+ static int16_t angleB=90;
-static int16_t angleC=0;
+ static int16_t angleC=0;
-static int16_t sum=2;
+ static int16_t sum=2;
 
-static int16_t m_pitch=0;
+ static int16_t m_pitch=0;
-static int16_t m_roll=0;
+ static int16_t m_roll=0;
-static int16_t m_yaw=0;
+ static int16_t m_yaw=0;
-static int16_t m_batt=0;
+ static int16_t m_batt=0;
-static int16_t m_alt=0;
+ static int16_t m_alt=0;
 
-static uint8_t m_gpsStatus=0;
+ static uint8_t m_gpsStatus=0;
-static int32_t m_gpsLat=0;
+ static int32_t m_gpsLat=0;
-static int32_t m_gpsLon=0;
+ static int32_t m_gpsLon=0;
-static float m_gpsAlt=0;
+ static float m_gpsAlt=0;
-static float m_gpsSpd=0;*/
+ static float m_gpsSpd=0;*/
 
 extern uint8_t *draw_buffer_level;
 extern uint8_t *draw_buffer_mask;
 extern uint8_t *disp_buffer_level;
 extern uint8_t *disp_buffer_mask;
 
-
 TTime timex;
 
 // ****************
@@ -97,9 +97,9 @@ static xTaskHandle osdgenTaskHandle;
 
 struct splashEntry
 {
-	unsigned int width, height;
+    unsigned int width, height;
-	const uint16_t *level;
+    const uint16_t *level;
-	const uint16_t *mask;
+    const uint16_t *mask;
 };
 
 struct splashEntry splash[3] = {
@@ -114,126 +114,126 @@ struct splashEntry splash[3] = {
 	{	llama_width,
 		llama_height,
 		llama_bits,
-		llama_mask_bits },
+		llama_mask_bits }
-
 };
 
 uint16_t mirror(uint16_t source)
 {
-	int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) |
+    int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1)
-			((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) |
+            | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5)
-			((source & 0x0800) << 1) | ((source & 0x0400) << 3) |
+            | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5)
-			((source & 0x0200) << 5) | ((source & 0x0100) << 7) |
+            | ((source & 0x0001) << 7);
-			((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) |
-			((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) |
-			((source & 0x0008) << 1) | ((source & 0x0004) << 3) |
-			((source & 0x0002) << 5) | ((source & 0x0001) << 7);
 
-	   return result;
+    return result;
 }
 
-void clearGraphics() {
+void clearGraphics()
-	memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
+{
-	memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
+    memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
+    memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
 }
 
-void copyimage(uint16_t offsetx, uint16_t offsety, int image) {
+void copyimage(uint16_t offsetx, uint16_t offsety, int image)
-	//check top/left position
+{
-	if (!validPos(offsetx, offsety)) {
+    //check top/left position
-		return;
+    if (!validPos(offsetx, offsety)) {
-	}
+        return;
-	struct splashEntry splash_info;
+    }
-	splash_info = splash[image];
+    struct splashEntry splash_info;
-	offsetx=offsetx/8;
+    splash_info = splash[image];
-	for (uint16_t y = offsety; y < ((splash_info.height)+offsety); y++) {
+    offsetx = offsetx / 8;
-		uint16_t x1=offsetx;
+    for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) {
-		for (uint16_t x = offsetx; x < (((splash_info.width)/16)+offsetx); x++) {
+        uint16_t x1 = offsetx;
-			draw_buffer_level[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8);
+        for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) {
-			draw_buffer_level[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
+            draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
-			draw_buffer_mask[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8);
+                    mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
-			draw_buffer_mask[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
+            draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)(
-			x1+=2;
+                    mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
-		}
+            draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
-	}
+                    mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
+            draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
+            x1 += 2;
+        }
+    }
 }
 
-uint8_t validPos(uint16_t x, uint16_t y) {
+uint8_t validPos(uint16_t x, uint16_t y)
-	if ( x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
+{
-		return 0;
+    if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
-	}
+        return 0;
-	return 1;
+    }
+    return 1;
 }
 
 // Credit for this one goes to wikipedia! :-)
-void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) {
+void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius)
-	  int f = 1 - radius;
+{
-	  int ddF_x = 1;
+    int f = 1 - radius;
-	  int ddF_y = -2 * radius;
+    int ddF_x = 1;
-	  int x = 0;
+    int ddF_y = -2 * radius;
-	  int y = radius;
+    int x = 0;
+    int y = radius;
 
-	  write_pixel_lm(x0, y0 + radius,1,1);
+    write_pixel_lm(x0, y0 + radius, 1, 1);
-	  write_pixel_lm(x0, y0 - radius,1,1);
+    write_pixel_lm(x0, y0 - radius, 1, 1);
-	  write_pixel_lm(x0 + radius, y0,1,1);
+    write_pixel_lm(x0 + radius, y0, 1, 1);
-	  write_pixel_lm(x0 - radius, y0,1,1);
+    write_pixel_lm(x0 - radius, y0, 1, 1);
 
-	  while(x < y)
+    while (x < y) {
-	  {
+        // ddF_x == 2 * x + 1;
-	    // ddF_x == 2 * x + 1;
+        // ddF_y == -2 * y;
-	    // ddF_y == -2 * y;
+        // f == x*x + y*y - radius*radius + 2*x - y + 1;
-	    // f == x*x + y*y - radius*radius + 2*x - y + 1;
+        if (f >= 0) {
-	    if(f >= 0)
+            y--;
-	    {
+            ddF_y += 2;
-	      y--;
+            f += ddF_y;
-	      ddF_y += 2;
+        }
-	      f += ddF_y;
+        x++;
-	    }
+        ddF_x += 2;
-	    x++;
+        f += ddF_x;
-	    ddF_x += 2;
+        write_pixel_lm(x0 + x, y0 + y, 1, 1);
-	    f += ddF_x;
+        write_pixel_lm(x0 - x, y0 + y, 1, 1);
-	    write_pixel_lm(x0 + x, y0 + y,1,1);
+        write_pixel_lm(x0 + x, y0 - y, 1, 1);
-	    write_pixel_lm(x0 - x, y0 + y,1,1);
+        write_pixel_lm(x0 - x, y0 - y, 1, 1);
-	    write_pixel_lm(x0 + x, y0 - y,1,1);
+        write_pixel_lm(x0 + y, y0 + x, 1, 1);
-	    write_pixel_lm(x0 - x, y0 - y,1,1);
+        write_pixel_lm(x0 - y, y0 + x, 1, 1);
-	    write_pixel_lm(x0 + y, y0 + x,1,1);
+        write_pixel_lm(x0 + y, y0 - x, 1, 1);
-	    write_pixel_lm(x0 - y, y0 + x,1,1);
+        write_pixel_lm(x0 - y, y0 - x, 1, 1);
-	    write_pixel_lm(x0 + y, y0 - x,1,1);
+    }
-	    write_pixel_lm(x0 - y, y0 - x,1,1);
-	  }
 }
 
-void swap(uint16_t* a, uint16_t* b) {
+void swap(uint16_t* a, uint16_t* b)
-	uint16_t temp = *a;
+{
-	*a = *b;
+    uint16_t temp = *a;
-	*b = temp;
+    *a = *b;
+    *b = temp;
 }
 
+static const int8_t sinData[91] =
+{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64,
+        66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98,
+        98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 };
 
-const static int8_t sinData[91] = {
+static int8_t mySin(uint16_t angle)
-  0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33,
+{
-  34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62,
+    uint16_t pos = 0;
-  63, 64, 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84,
+    pos = angle % 360;
-  85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97,
+    int8_t mult = 1;
-  97, 98, 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100};
+    // 180-359 is same as 0-179 but negative.
-
+    if (pos >= 180) {
-static int8_t mySin(uint16_t angle) {
+        pos = pos - 180;
-	uint16_t pos = 0;
+        mult = -1;
-	pos = angle % 360;
+    }
-	int8_t mult = 1;
+    // 0-89 is equal to 90-179 except backwards.
-	// 180-359 is same as 0-179 but negative.
+    if (pos >= 90) {
-	if (pos >= 180) {
+        pos = 180 - pos;
-		pos = pos - 180;
+    }
-		mult = -1;
+    return mult * (int8_t)(sinData[pos]);
-	}
-	// 0-89 is equal to 90-179 except backwards.
-	if (pos >= 90) {
-		pos = 180 - pos;
-	}
-	return mult * (int8_t)(sinData[pos]);
 }
 
-static int8_t myCos(uint16_t angle) {
+static int8_t myCos(uint16_t angle)
-	return mySin(angle + 90);
+{
+    return mySin(angle + 90);
 }
 
 /// Draws four points relative to the given center point.
@@ -250,10 +250,10 @@ static int8_t myCos(uint16_t angle) {
 /// \param color the color to draw the pixels with.
 void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY)
 {
-	write_pixel_lm(centerX + deltaX, centerY + deltaY,1,1);      // Ist      Quadrant
+    write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1);      // Ist      Quadrant
-	write_pixel_lm(centerX - deltaX, centerY + deltaY,1,1);      // IInd     Quadrant
+    write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1);      // IInd     Quadrant
-	write_pixel_lm(centerX - deltaX, centerY - deltaY,1,1);      // IIIrd    Quadrant
+    write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1);      // IIIrd    Quadrant
-	write_pixel_lm(centerX + deltaX, centerY - deltaY,1,1);      // IVth     Quadrant
+    write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1);      // IVth     Quadrant
 }
 
 /// Implements the midpoint ellipse drawing algorithm which is a bresenham
@@ -278,62 +278,57 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
 
     plotFourQuadrants(centerX, centerY, x, y);
 
-    while(deltaY >= deltaX)
+    while (deltaY >= deltaX) {
-    {
+        x++;
-          x++;
+        deltaX += (doubleVerticalRadius << 1);
-          deltaX += (doubleVerticalRadius << 1);
 
-          error +=  deltaX + doubleVerticalRadius;
+        error += deltaX + doubleVerticalRadius;
 
-          if(error >= 0)
+        if (error >= 0) {
-          {
+            y--;
-               y--;
+            deltaY -= (doubleHorizontalRadius << 1);
-               deltaY -= (doubleHorizontalRadius << 1);
 
-               error -= deltaY;
+            error -= deltaY;
-          }
+        }
-          plotFourQuadrants(centerX, centerY, x, y);
+        plotFourQuadrants(centerX, centerY, x, y);
     }
 
     error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0f) * (x + 1 / 2.0f) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
 
-    while (y>=0)
+    while (y >= 0) {
-    {
+        error += doubleHorizontalRadius;
-          error += doubleHorizontalRadius;
+        y--;
-          y--;
+        deltaY -= (doubleHorizontalRadius << 1);
-          deltaY -= (doubleHorizontalRadius<<1);
+        error -= deltaY;
-          error -= deltaY;
 
-          if(error <= 0)
+        if (error <= 0) {
-          {
+            x++;
-               x++;
+            deltaX += (doubleVerticalRadius << 1);
-               deltaX += (doubleVerticalRadius << 1);
+            error += deltaX;
-               error += deltaX;
+        }
-          }
 
-          plotFourQuadrants(centerX, centerY, x, y);
+        plotFourQuadrants(centerX, centerY, x, y);
     }
 }
 
-
 void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size)
 {
-	int16_t a = myCos(angle);
+    int16_t a = myCos(angle);
-	int16_t b = mySin(angle);
+    int16_t b = mySin(angle);
-	a = (a * (size/2)) / 100;
+    a = (a * (size / 2)) / 100;
-	b = (b * (size/2)) / 100;
+    b = (b * (size / 2)) / 100;
-	write_line_lm((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a, 1, 1); //Direction line
+    write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); //Direction line
-	//write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line
+    //write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line
-	write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 - a/2, (y)-1 - b/2, 1, 1); // Arrow "wings"
+    write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings"
-	write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 + a/2, (y)-1 + b/2, 1, 1);
+    write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 1, 1);
 }
 
 void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
 {
-	write_line_lm(x1, y1, x2, y1, 1, 1); //top
+    write_line_lm(x1, y1, x2, y1, 1, 1); //top
-	write_line_lm(x1, y1, x1, y2, 1, 1); //left
+    write_line_lm(x1, y1, x1, y2, 1, 1); //left
-	write_line_lm(x2, y1, x2, y2, 1, 1); //right
+    write_line_lm(x2, y1, x2, y2, 1, 1); //right
-	write_line_lm(x1, y2, x2, y2, 1, 1); //bottom
+    write_line_lm(x1, y2, x2, y2, 1, 1); //bottom
 }
 
 // simple routines
@@ -350,14 +345,14 @@ void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
  */
 void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode)
 {
-	CHECK_COORDS(x, y);
+    CHECK_COORDS(x, y);
-	// Determine the bit in the word to be set and the word
+    // Determine the bit in the word to be set and the word
-	// index to set it in.
+    // index to set it in.
-	int bitnum = CALC_BIT_IN_WORD(x);
+    int bitnum = CALC_BIT_IN_WORD(x);
-	int wordnum = CALC_BUFF_ADDR(x, y);
+    int wordnum = CALC_BUFF_ADDR(x, y);
-	// Apply a mask.
+    // Apply a mask.
-	uint16_t mask = 1 << (7 - bitnum);
+    uint16_t mask = 1 << (7 - bitnum);
-	WRITE_WORD_MODE(buff, wordnum, mask, mode);
+    WRITE_WORD_MODE(buff, wordnum, mask, mode);
 }
 
 /**
@@ -371,18 +366,17 @@ void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode)
  */
 void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
 {
-	CHECK_COORDS(x, y);
+    CHECK_COORDS(x, y);
-	// Determine the bit in the word to be set and the word
+    // Determine the bit in the word to be set and the word
-	// index to set it in.
+    // index to set it in.
-	int bitnum = CALC_BIT_IN_WORD(x);
+    int bitnum = CALC_BIT_IN_WORD(x);
-	int wordnum = CALC_BUFF_ADDR(x, y);
+    int wordnum = CALC_BUFF_ADDR(x, y);
-	// Apply the masks.
+    // Apply the masks.
-	uint16_t mask = 1 << (7 - bitnum);
+    uint16_t mask = 1 << (7 - bitnum);
-	WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode);
+    WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode);
-	WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
+    WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
 }
 
-
 /**
  * write_hline: optimised horizontal line writing algorithm
  *
@@ -394,41 +388,38 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
  */
 void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y, int mode)
 {
-	CLIP_COORDS(x0, y);
+    CLIP_COORDS(x0, y);
-	CLIP_COORDS(x1, y);
+    CLIP_COORDS(x1, y);
-	if(x0 > x1)
+    if (x0 > x1) {
-	{
+        SWAP(x0, x1);
-		SWAP(x0, x1);
+    }
-	}
+    if (x0 == x1) {
-	if(x0 == x1) return;
+        return;
-	/* This is an optimised algorithm for writing horizontal lines.
+    }
-	 * We begin by finding the addresses of the x0 and x1 points. */
+    /* This is an optimised algorithm for writing horizontal lines.
-	int addr0 = CALC_BUFF_ADDR(x0, y);
+     * We begin by finding the addresses of the x0 and x1 points. */
-	int addr1 = CALC_BUFF_ADDR(x1, y);
+    int addr0 = CALC_BUFF_ADDR(x0, y);
-	int addr0_bit = CALC_BIT_IN_WORD(x0);
+    int addr1 = CALC_BUFF_ADDR(x1, y);
-	int addr1_bit = CALC_BIT_IN_WORD(x1);
+    int addr0_bit = CALC_BIT_IN_WORD(x0);
-	int mask, mask_l, mask_r, i;
+    int addr1_bit = CALC_BIT_IN_WORD(x1);
-	/* If the addresses are equal, we only need to write one word
+    int mask, mask_l, mask_r, i;
-	 * which is an island. */
+    /* If the addresses are equal, we only need to write one word
-	if(addr0 == addr1)
+     * which is an island. */
-	{
+    if (addr0 == addr1) {
-		mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
+        mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
-		WRITE_WORD_MODE(buff, addr0, mask, mode);
+        WRITE_WORD_MODE(buff, addr0, mask, mode);
-	}
+    } else {
-	/* Otherwise we need to write the edges and then the middle. */
+        /* Otherwise we need to write the edges and then the middle. */
-	else
+        mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
-	{
+        mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
-		mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
+        WRITE_WORD_MODE(buff, addr0, mask_l, mode);
-		mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
+        WRITE_WORD_MODE(buff, addr1, mask_r, mode);
-		WRITE_WORD_MODE(buff, addr0, mask_l, mode);
+        // Now write 0xffff words from start+1 to end-1.
-		WRITE_WORD_MODE(buff, addr1, mask_r, mode);
+        for (i = addr0 + 1; i <= addr1 - 1; i++) {
-		// Now write 0xffff words from start+1 to end-1.
+            uint8_t m = 0xff;
-		for(i = addr0 + 1; i <= addr1 - 1; i++)
+            WRITE_WORD_MODE(buff, i, m, mode);
-		{
+        }
-			uint8_t m=0xff;
+    }
-			WRITE_WORD_MODE(buff, i, m, mode);
-		}
-	}
 }
 
 /**
@@ -442,10 +433,10 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
  */
 void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, int mmode)
 {
-	// TODO: an optimisation would compute the masks and apply to
+    // TODO: an optimisation would compute the masks and apply to
-	// both buffers simultaneously.
+    // both buffers simultaneously.
-	write_hline(draw_buffer_level, x0, x1, y, lmode);
+    write_hline(draw_buffer_level, x0, x1, y, lmode);
-	write_hline(draw_buffer_mask, x0, x1, y, mmode);
+    write_hline(draw_buffer_mask, x0, x1, y, mmode);
 }
 
 /**
@@ -462,19 +453,18 @@ void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode,
  */
 void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int endcap0, int endcap1, int mode, int mmode)
 {
-	int stroke, fill;
+    int stroke, fill;
-	SETUP_STROKE_FILL(stroke, fill, mode)
+    SETUP_STROKE_FILL(stroke, fill, mode)
-	if(x0 > x1)
+    if (x0 > x1) {
-	{
+        SWAP(x0, x1);
-		SWAP(x0, x1);
+    }
-	}
+    // Draw the main body of the line.
-	// Draw the main body of the line.
+    write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode);
-	write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode);
+    write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode);
-	write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode);
+    write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode);
-	write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode);
+    // Draw the endcaps, if any.
-	// Draw the endcaps, if any.
+    DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode);
-	DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode);
+    DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode);
-	DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode);
 }
 
 /**
@@ -488,27 +478,27 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int
  */
 void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode)
 {
-	unsigned int a;
+    unsigned int a;
-	CLIP_COORDS(x, y0);
+    CLIP_COORDS(x, y0);
-	CLIP_COORDS(x, y1);
+    CLIP_COORDS(x, y1);
-	if(y0 > y1)
+    if (y0 > y1) {
-	{
+        SWAP(y0, y1);
-		SWAP(y0, y1);
+    }
-	}
+    if (y0 == y1) {
-	if(y0 == y1) return;
+        return;
-	/* This is an optimised algorithm for writing vertical lines.
+    }
-	 * We begin by finding the addresses of the x,y0 and x,y1 points. */
+    /* This is an optimised algorithm for writing vertical lines.
-	int addr0 = CALC_BUFF_ADDR(x, y0);
+     * We begin by finding the addresses of the x,y0 and x,y1 points. */
-	int addr1 = CALC_BUFF_ADDR(x, y1);
+    unsigned int addr0 = CALC_BUFF_ADDR(x, y0);
-	/* Then we calculate the pixel data to be written. */
+    unsigned int addr1 = CALC_BUFF_ADDR(x, y1);
-	int bitnum = CALC_BIT_IN_WORD(x);
+    /* Then we calculate the pixel data to be written. */
-	uint16_t mask = 1 << (7 - bitnum);
+    unsigned int bitnum = CALC_BIT_IN_WORD(x);
-	/* Run from addr0 to addr1 placing pixels. Increment by the number
+    uint16_t mask = 1 << (7 - bitnum);
-	 * of words n each graphics line. */
+    /* Run from addr0 to addr1 placing pixels. Increment by the number
-	for(a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8)
+     * of words n each graphics line. */
-	{
+    for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) {
-		WRITE_WORD_MODE(buff, a, mask, mode);
+        WRITE_WORD_MODE(buff, a, mask, mode);
-	}
+    }
 }
 
 /**
@@ -522,10 +512,10 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
  */
 void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, int mmode)
 {
-	// TODO: an optimisation would compute the masks and apply to
+    // TODO: an optimisation would compute the masks and apply to
-	// both buffers simultaneously.
+    // both buffers simultaneously.
-	write_vline(draw_buffer_level, x, y0, y1, lmode);
+    write_vline(draw_buffer_level, x, y0, y1, lmode);
-	write_vline(draw_buffer_mask, x, y0, y1, mmode);
+    write_vline(draw_buffer_mask, x, y0, y1, mmode);
 }
 
 /**
@@ -542,19 +532,18 @@ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode,
  */
 void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
 {
-	int stroke, fill;
+    int stroke, fill;
-	if(y0 > y1)
+    if (y0 > y1) {
-	{
+        SWAP(y0, y1);
-		SWAP(y0, y1);
+    }
-	}
+    SETUP_STROKE_FILL(stroke, fill, mode);
-	SETUP_STROKE_FILL(stroke, fill, mode);
+    // Draw the main body of the line.
-	// Draw the main body of the line.
+    write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode);
-	write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode);
+    write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode);
-	write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode);
+    write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode);
-	write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode);
+    // Draw the endcaps, if any.
-	// Draw the endcaps, if any.
+    DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode);
-	DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode);
+    DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode);
-	DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode);
 }
 
 /**
@@ -573,61 +562,56 @@ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int
  */
 void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode)
 {
-        int yy, addr0_old, addr1_old;
+    unsigned int yy, addr0_old, addr1_old;
-        CHECK_COORDS(x, y);
+    CHECK_COORDS(x, y);
-        CHECK_COORD_X(x + width);
+    CHECK_COORD_X(x + width);
-        CHECK_COORD_Y(y + height);
+    CHECK_COORD_Y(y + height);
-        if(width <= 0 || height <= 0) return;
+    if (width <= 0 || height <= 0) {
-        // Calculate as if the rectangle was only a horizontal line. We then
+        return;
-        // step these addresses through each row until we iterate `height` times.
+    }
-        int addr0 = CALC_BUFF_ADDR(x, y);
+    // Calculate as if the rectangle was only a horizontal line. We then
-        int addr1 = CALC_BUFF_ADDR(x + width, y);
+    // step these addresses through each row until we iterate `height` times.
-        int addr0_bit = CALC_BIT_IN_WORD(x);
+    unsigned int addr0 = CALC_BUFF_ADDR(x, y);
-        int addr1_bit = CALC_BIT_IN_WORD(x + width);
+    unsigned int addr1 = CALC_BUFF_ADDR(x + width, y);
-        int mask, mask_l, mask_r, i;
+    unsigned int addr0_bit = CALC_BIT_IN_WORD(x);
-        // If the addresses are equal, we need to write one word vertically.
+    unsigned int addr1_bit = CALC_BIT_IN_WORD(x + width);
-        if(addr0 == addr1)
+    unsigned int mask, mask_l, mask_r, i;
-        {
+    // If the addresses are equal, we need to write one word vertically.
-                mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
+    if (addr0 == addr1) {
-                while(height--)
+        mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
-                {
+        while (height--) {
-                        WRITE_WORD_MODE(buff, addr0, mask, mode);
+            WRITE_WORD_MODE(buff, addr0, mask, mode);
-                        addr0 += GRAPHICS_WIDTH_REAL / 8;
+            addr0 += GRAPHICS_WIDTH_REAL / 8;
-                }
         }
+    } else {
         // Otherwise we need to write the edges and then the middle repeatedly.
-        else
+        mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
-        {
+        mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
-                mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
+        // Write edges first.
-                mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
+        yy = 0;
-                // Write edges first.
+        addr0_old = addr0;
-                yy = 0;
+        addr1_old = addr1;
-                addr0_old = addr0;
+        while (yy < height) {
-                addr1_old = addr1;
+            WRITE_WORD_MODE(buff, addr0, mask_l, mode);
-                while(yy < height)
+            WRITE_WORD_MODE(buff, addr1, mask_r, mode);
-                {
+            addr0 += GRAPHICS_WIDTH_REAL / 8;
-                        WRITE_WORD_MODE(buff, addr0, mask_l, mode);
+            addr1 += GRAPHICS_WIDTH_REAL / 8;
-                        WRITE_WORD_MODE(buff, addr1, mask_r, mode);
+            yy++;
-                        addr0 += GRAPHICS_WIDTH_REAL / 8;
-                        addr1 += GRAPHICS_WIDTH_REAL / 8;
-                        yy++;
-                }
-                // Now write 0xffff words from start+1 to end-1 for each row.
-                yy = 0;
-                addr0 = addr0_old;
-                addr1 = addr1_old;
-                while(yy < height)
-                {
-                        for(i = addr0 + 1; i <= addr1 - 1; i++)
-                        {
-                        	uint8_t m=0xff;
-                            WRITE_WORD_MODE(buff, i, m, mode);
-                        }
-                        addr0 += GRAPHICS_WIDTH_REAL / 8;
-                        addr1 += GRAPHICS_WIDTH_REAL / 8;
-                        yy++;
-                }
         }
+        // Now write 0xffff words from start+1 to end-1 for each row.
+        yy = 0;
+        addr0 = addr0_old;
+        addr1 = addr1_old;
+        while (yy < height) {
+            for (i = addr0 + 1; i <= addr1 - 1; i++) {
+                uint8_t m = 0xff;
+                WRITE_WORD_MODE(buff, i, m, mode);
+            }
+            addr0 += GRAPHICS_WIDTH_REAL / 8;
+            addr1 += GRAPHICS_WIDTH_REAL / 8;
+            yy++;
+        }
+    }
 }
 
 /**
@@ -642,8 +626,8 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
  */
 void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int width, unsigned int height, int lmode, int mmode)
 {
-        write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode);
+    write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode);
-        write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode);
+    write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode);
 }
 
 /**
@@ -659,14 +643,14 @@ void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int widt
  */
 void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode)
 {
-	//CHECK_COORDS(x, y);
+    //CHECK_COORDS(x, y);
-	//CHECK_COORDS(x + width, y + height);
+    //CHECK_COORDS(x + width, y + height);
-	//if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x;
+    //if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x;
-	//if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y;
+    //if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y;
-	write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
+    write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
-	write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
+    write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
-	write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
+    write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
-	write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
+    write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode);
 }
 
 /**
@@ -682,22 +666,19 @@ void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int hei
  */
 void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int mode)
 {
-	CHECK_COORDS(cx, cy);
+    CHECK_COORDS(cx, cy);
-	int error = -r, x = r, y = 0;
+    int error = -r, x = r, y = 0;
-	while(x >= y)
+    while (x >= y) {
-	{
+        if (dashp == 0 || (y % dashp) < (dashp / 2)) {
-		if(dashp == 0 || (y % dashp) < (dashp / 2))
+            CIRCLE_PLOT_8(buff, cx, cy, x, y, mode);
-		{
+        }
-			CIRCLE_PLOT_8(buff, cx, cy, x, y, mode);
+        error += (y * 2) + 1;
-		}
+        y++;
-		error += (y * 2) + 1;
+        if (error >= 0) {
-		y++;
+            --x;
-		if(error >= 0)
+            error -= x * 2;
-		{
+        }
-			--x;
+    }
-			error -= x * 2;
-		}
-	}
 }
 
 /**
@@ -713,58 +694,51 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int
  */
 void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int bmode, int mode, int mmode)
 {
-	int stroke, fill;
+    int stroke, fill;
-	CHECK_COORDS(cx, cy);
+    CHECK_COORDS(cx, cy);
-	SETUP_STROKE_FILL(stroke, fill, mode);
+    SETUP_STROKE_FILL(stroke, fill, mode);
-	// This is a two step procedure. First, we draw the outline of the
+    // This is a two step procedure. First, we draw the outline of the
-	// circle, then we draw the inner part.
+    // circle, then we draw the inner part.
-	int error = -r, x = r, y = 0;
+    int error = -r, x = r, y = 0;
-	while(x >= y)
+    while (x >= y) {
-	{
+        if (dashp == 0 || (y % dashp) < (dashp / 2)) {
-		if(dashp == 0 || (y % dashp) < (dashp / 2))
+            CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode);
-		{
+            CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
-			CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode);
+            CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
-			CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
+            CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke);
-			CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
+            CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode);
-			CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke);
+            CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
-			CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode);
+            CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
-			CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
+            CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
-			CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
+            if (bmode == 1) {
-			CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
+                CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode);
-			if(bmode == 1)
+                CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke);
-			{
+                CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode);
-				CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode);
+                CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke);
-				CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke);
+            }
-				CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode);
+        }
-				CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke);
+        error += (y * 2) + 1;
-			}
+        y++;
-		}
+        if (error >= 0) {
-		error += (y * 2) + 1;
+            --x;
-		y++;
+            error -= x * 2;
-		if(error >= 0)
+        }
-		{
+    }
-			--x;
+    error = -r;
-			error -= x * 2;
+    x = r;
-		}
+    y = 0;
-	}
+    while (x >= y) {
-	error = -r;
+        if (dashp == 0 || (y % dashp) < (dashp / 2)) {
-	x = r;
+            CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
-	y = 0;
+            CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
-	while(x >= y)
+        }
-	{
+        error += (y * 2) + 1;
-		if(dashp == 0 || (y % dashp) < (dashp / 2))
+        y++;
-		{
+        if (error >= 0) {
-			CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
+            --x;
-			CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
+            error -= x * 2;
-		}
+        }
-		error += (y * 2) + 1;
+    }
-		y++;
-		if(error >= 0)
-		{
-			--x;
-			error -= x * 2;
-		}
-	}
 }
 
 /**
@@ -778,42 +752,37 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
  */
 void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, int mode)
 {
-	CHECK_COORDS(cx, cy);
+    CHECK_COORDS(cx, cy);
-	int error = -r, x = r, y = 0, xch = 0;
+    int error = -r, x = r, y = 0, xch = 0;
-	// It turns out that filled circles can take advantage of the midpoint
+    // It turns out that filled circles can take advantage of the midpoint
-	// circle algorithm. We simply draw very fast horizontal lines across each
+    // circle algorithm. We simply draw very fast horizontal lines across each
-	// pair of X,Y coordinates. In some cases, this can even be faster than
+    // pair of X,Y coordinates. In some cases, this can even be faster than
-	// drawing an outlined circle!
+    // drawing an outlined circle!
-	//
+    //
-	// Due to multiple writes to each set of pixels, we have a special exception
+    // Due to multiple writes to each set of pixels, we have a special exception
-	// for when using the toggling draw mode.
+    // for when using the toggling draw mode.
-	while(x >= y)
+    while (x >= y) {
-	{
+        if (y != 0) {
-		if(y != 0)
+            write_hline(buff, cx - x, cx + x, cy + y, mode);
-		{
+            write_hline(buff, cx - x, cx + x, cy - y, mode);
-			write_hline(buff, cx - x, cx + x, cy + y, mode);
+            if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) {
-			write_hline(buff, cx - x, cx + x, cy - y, mode);
+                write_hline(buff, cx - y, cx + y, cy + x, mode);
-			if(mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y)))
+                write_hline(buff, cx - y, cx + y, cy - x, mode);
-			{
+                xch = 0;
-				write_hline(buff, cx - y, cx + y, cy + x, mode);
+            }
-				write_hline(buff, cx - y, cx + y, cy - x, mode);
+        }
-				xch = 0;
+        error += (y * 2) + 1;
-			}
+        y++;
-		}
+        if (error >= 0) {
-		error += (y * 2) + 1;
+            --x;
-		y++;
+            xch = 1;
-		if(error >= 0)
+            error -= x * 2;
-		{
+        }
-			--x;
+    }
-			xch = 1;
+    // Handle toggle mode.
-			error -= x * 2;
+    if (mode == 2) {
-		}
+        write_hline(buff, cx - r, cx + r, cy, mode);
-	}
+    }
-	// Handle toggle mode.
-	if(mode == 2)
-	{
-		write_hline(buff, cx - r, cx + r, cy, mode);
-	}
 }
 
 /**
@@ -828,45 +797,39 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
  */
 void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode)
 {
-	// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+    // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
-	int steep = abs(y1 - y0) > abs(x1 - x0);
+	unsigned int steep = abs(y1 - y0) > abs(x1 - x0);
-	if(steep)
+    if (steep) {
-	{
+        SWAP(x0, y0);
-		SWAP(x0, y0);
+        SWAP(x1, y1);
-		SWAP(x1, y1);
+    }
-	}
+    if (x0 > x1) {
-	if(x0 > x1)
+        SWAP(x0, x1);
-	{
+        SWAP(y0, y1);
-		SWAP(x0, x1);
+    }
-		SWAP(y0, y1);
+    int deltax = x1 - x0;
-	}
+    unsigned int deltay = abs(y1 - y0);
-	int deltax = x1 - x0;
+    int error = deltax / 2;
-	int deltay = abs(y1 - y0);
+    int ystep;
-	int error = deltax / 2;
+    unsigned int y = y0;
-	int ystep;
+    unsigned int x; //, lasty = y, stox = 0;
-	int y = y0;
+    if (y0 < y1) {
-	int x; //, lasty = y, stox = 0;
+        ystep = 1;
-	if(y0 < y1)
+    } else {
-		ystep = 1;
+        ystep = -1;
-	else
+    }
-		ystep = -1;
+    for (x = x0; x < x1; x++) {
-	for(x = x0; x < x1; x++)
+        if (steep) {
-	{
+            write_pixel(buff, y, x, mode);
-		if(steep)
+        } else {
-		{
+            write_pixel(buff, x, y, mode);
-			write_pixel(buff, y, x, mode);
+        }
-		}
+        error -= deltay;
-		else
+        if (error < 0) {
-		{
+            y += ystep;
-			write_pixel(buff, x, y, mode);
+            error += deltax;
-		}
+        }
-		error -= deltay;
+    }
-		if(error < 0)
-		{
-			y += ystep;
-			error += deltax;
-		}
-	}
 }
 
 /**
@@ -881,8 +844,8 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
  */
 void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mmode, int lmode)
 {
-	write_line(draw_buffer_mask, x0, y0, x1, y1, mmode);
+    write_line(draw_buffer_mask, x0, y0, x1, y1, mmode);
-	write_line(draw_buffer_level, x0, y0, x1, y1, lmode);
+    write_line(draw_buffer_level, x0, y0, x1, y1, lmode);
 }
 
 /**
@@ -898,86 +861,74 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i
  * @param       mode            0 = black outline, white body, 1 = white outline, black body
  * @param       mmode           0 = clear, 1 = set, 2 = toggle
  */
-void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
+void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1,
+							__attribute__((unused)) int endcap0, __attribute__((unused)) int endcap1,
+							int mode, int mmode)
 {
-	// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+    // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
-	// This could be improved for speed.
+    // This could be improved for speed.
-	int omode, imode;
+    int omode, imode;
-	if(mode == 0)
+    if (mode == 0) {
-	{
+        omode = 0;
-		omode = 0;
+        imode = 1;
-		imode = 1;
+    } else {
-	}
+        omode = 1;
-	else
+        imode = 0;
-	{
+    }
-		omode = 1;
+    int steep = abs(y1 - y0) > abs(x1 - x0);
-		imode = 0;
+    if (steep) {
-	}
+        SWAP(x0, y0);
-	int steep = abs(y1 - y0) > abs(x1 - x0);
+        SWAP(x1, y1);
-	if(steep)
+    }
-	{
+    if (x0 > x1) {
-		SWAP(x0, y0);
+        SWAP(x0, x1);
-		SWAP(x1, y1);
+        SWAP(y0, y1);
-	}
+    }
-	if(x0 > x1)
+    int deltax = x1 - x0;
-	{
+    unsigned int deltay = abs(y1 - y0);
-		SWAP(x0, x1);
+    int error = deltax / 2;
-		SWAP(y0, y1);
+    int ystep;
-	}
+    unsigned int y = y0;
-	int deltax = x1 - x0;
+    unsigned int x;
-	int deltay = abs(y1 - y0);
+    if (y0 < y1) {
-	int error = deltax / 2;
+        ystep = 1;
-	int ystep;
+    } else {
-	int y = y0;
+        ystep = -1;
-	int x;
+    }
-	if(y0 < y1)
+    // Draw the outline.
-		ystep = 1;
+    for (x = x0; x < x1; x++) {
-	else
+        if (steep) {
-		ystep = -1;
+            write_pixel_lm(y - 1, x, mmode, omode);
-	// Draw the outline.
+            write_pixel_lm(y + 1, x, mmode, omode);
-	for(x = x0; x < x1; x++)
+            write_pixel_lm(y, x - 1, mmode, omode);
-	{
+            write_pixel_lm(y, x + 1, mmode, omode);
-		if(steep)
+        } else {
-		{
+            write_pixel_lm(x - 1, y, mmode, omode);
-			write_pixel_lm(y - 1, x, mmode, omode);
+            write_pixel_lm(x + 1, y, mmode, omode);
-			write_pixel_lm(y + 1, x, mmode, omode);
+            write_pixel_lm(x, y - 1, mmode, omode);
-			write_pixel_lm(y, x - 1, mmode, omode);
+            write_pixel_lm(x, y + 1, mmode, omode);
-			write_pixel_lm(y, x + 1, mmode, omode);
+        }
-		}
+        error -= deltay;
-		else
+        if (error < 0) {
-		{
+            y += ystep;
-			write_pixel_lm(x - 1, y, mmode, omode);
+            error += deltax;
-			write_pixel_lm(x + 1, y, mmode, omode);
+        }
-			write_pixel_lm(x, y - 1, mmode, omode);
+    }
-			write_pixel_lm(x, y + 1, mmode, omode);
+    // Now draw the innards.
-		}
+    error = deltax / 2;
-		error -= deltay;
+    y = y0;
-		if(error < 0)
+    for (x = x0; x < x1; x++) {
-		{
+        if (steep) {
-			y += ystep;
+            write_pixel_lm(y, x, mmode, imode);
-			error += deltax;
+        } else {
-		}
+            write_pixel_lm(x, y, mmode, imode);
-	}
+        }
-	// Now draw the innards.
+        error -= deltay;
-	error = deltax / 2;
+        if (error < 0) {
-	y = y0;
+            y += ystep;
-	for(x = x0; x < x1; x++)
+            error += deltax;
-	{
+        }
-		if(steep)
+    }
-		{
-			write_pixel_lm(y, x, mmode, imode);
-		}
-		else
-		{
-			write_pixel_lm(x, y, mmode, imode);
-		}
-		error -= deltay;
-		if(error < 0)
-		{
-			y += ystep;
-			error += deltax;
-		}
-	}
 }
 
 /**
@@ -994,12 +945,13 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
  */
 void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode)
 {
-	uint16_t firstmask = word >> xoff;
+    int16_t firstmask = word >> xoff;
-	uint16_t lastmask = word << (16 - xoff);
+    int16_t lastmask = word << (16 - xoff);
-	WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode);
+    WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode);
-	WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode);
+    WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode);
-	if(xoff > 0)
+    if (xoff > 0) {
-		WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode);
+        WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode);
+    }
 }
 
 /**
@@ -1019,12 +971,13 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi
  */
 void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
 {
-	uint16_t firstmask = word >> xoff;
+    uint16_t firstmask = word >> xoff;
-	uint16_t lastmask = word << (16 - xoff);
+    uint16_t lastmask = word << (16 - xoff);
-	WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff);
+    WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff);
-	WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
+    WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
-	if(xoff > 0)
+    if (xoff > 0) {
-		WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8);
+        WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8);
+    }
 }
 
 /**
@@ -1044,12 +997,13 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr,
  */
 void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff)
 {
-	uint16_t firstmask = word >> xoff;
+    uint16_t firstmask = word >> xoff;
-	uint16_t lastmask = word << (16 - xoff);
+    uint16_t lastmask = word << (16 - xoff);
-	WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff);
+    WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff);
-	WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
+    WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
-	if(xoff > 0)
+    if (xoff > 0) {
-		WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
+        WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
+    }
 
 }
 
@@ -1067,8 +1021,8 @@ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, u
  */
 void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode)
 {
-	write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode);
+    write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode);
-	write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode);
+    write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode);
 }
 
 /**
@@ -1079,22 +1033,22 @@ void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr,
  */
 int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup)
 {
-	// First locate the font struct.
+    // First locate the font struct.
-	if(font > SIZEOF_ARRAY(fonts))
+    if ((unsigned int)font > SIZEOF_ARRAY(fonts)) {
-		return 0; // font does not exist, exit.*/
+        return 0; // font does not exist, exit.
-	// Load the font info; IDs are always sequential.
+    }
-	*font_info = fonts[font];
+    // Load the font info; IDs are always sequential.
-	// Locate character in font lookup table. (If required.)
+    *font_info = fonts[font];
-	if(lookup != NULL)
+    // Locate character in font lookup table. (If required.)
-	{
+    if (lookup != NULL) {
-		*lookup = font_info->lookup[ch];
+        *lookup = font_info->lookup[ch];
-		if(*lookup == 0xff)
+        if (*lookup == 0xff) {
-			return 0; // character doesn't exist, don't bother writing it.
+            return 0; // character doesn't exist, don't bother writing it.
-	}
+        }
-	return 1;
+    }
+    return 1;
 }
 
-
 /**
  * write_char16: Draw a character on the current draw buffer.
  * Currently supports outlined characters and characters with
@@ -1108,12 +1062,12 @@ int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *loo
  */
 void write_char16(char ch, unsigned int x, unsigned int y, int font)
 {
-    int yy, addr_temp, row, row_temp, xshift;
+    unsigned int yy, addr_temp, row, row_temp, xshift;
     uint16_t and_mask, or_mask, level_bits;
     struct FontEntry font_info;
     //char lookup = 0;
     fetch_font_info(0, font, &font_info, NULL);
-	
+
     // Compute starting address (for x,y) of character.
     int addr = CALC_BUFF_ADDR(x, y);
     int wbit = CALC_BIT_IN_WORD(x);
@@ -1122,58 +1076,55 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
     // How big is the character? We handle characters up to 8 pixels
     // wide for now. Support for large characters may be added in future.
     {
-		// Ensure we don't overflow.
+        // Ensure we don't overflow.
-		if(x + wbit > GRAPHICS_WIDTH_REAL)
+        if (x + wbit > GRAPHICS_WIDTH_REAL) {
-			return;
+            return;
-		// Load data pointer.
+        }
-		row = ch * font_info.height;
+        // Load data pointer.
-		row_temp = row;
+        row = ch * font_info.height;
-		addr_temp = addr;
+        row_temp = row;
-		xshift = 16 - font_info.width;
+        addr_temp = addr;
-		// We can write mask words easily.
+        xshift = 16 - font_info.width;
-		for(yy = y; yy < y + font_info.height; yy++)
+        // We can write mask words easily.
-		{
+        for (yy = y; yy < y + font_info.height; yy++) {
-			if(font==3)
+            if (font == 3) {
-				write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit);
+                write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit);
-			else
+            } else {
-				write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
+                write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
-			addr += GRAPHICS_WIDTH_REAL / 8;
+            }
-			row++;
+            addr += GRAPHICS_WIDTH_REAL / 8;
-		}
+            row++;
-		// Level bits are more complicated. We need to set or clear
+        }
-		// level bits, but only where the mask bit is set; otherwise,
+        // Level bits are more complicated. We need to set or clear
-		// we need to leave them alone. To do this, for each word, we
+        // level bits, but only where the mask bit is set; otherwise,
-		// construct an AND mask and an OR mask, and apply each individually.
+        // we need to leave them alone. To do this, for each word, we
-		row = row_temp;
+        // construct an AND mask and an OR mask, and apply each individually.
-		addr = addr_temp;
+        row = row_temp;
-		for(yy = y; yy < y + font_info.height; yy++)
+        addr = addr_temp;
-		{
+        for (yy = y; yy < y + font_info.height; yy++) {
-			if(font==3)
+            if (font == 3) {
-			{
+                level_bits = font_frame12x18[row];
-				level_bits = font_frame12x18[row];
+                //if(!(flags & FONT_INVERT)) // data is normally inverted
-				//if(!(flags & FONT_INVERT)) // data is normally inverted
+                level_bits = ~level_bits;
-				level_bits = ~level_bits;
+                or_mask = font_mask12x18[row] << xshift;
-				or_mask = font_mask12x18[row] << xshift;
+                and_mask = (font_mask12x18[row] & level_bits) << xshift;
-				and_mask = (font_mask12x18[row] & level_bits) << xshift;
+            } else {
-			} else {
+                level_bits = font_frame8x10[row];
-				level_bits = font_frame8x10[row];
+                //if(!(flags & FONT_INVERT)) // data is normally inverted
-				//if(!(flags & FONT_INVERT)) // data is normally inverted
+                level_bits = ~level_bits;
-				level_bits = ~level_bits;
+                or_mask = font_mask8x10[row] << xshift;
-				or_mask = font_mask8x10[row] << xshift;
+                and_mask = (font_mask8x10[row] & level_bits) << xshift;
-				and_mask = (font_mask8x10[row] & level_bits) << xshift;
+            }
-			}
+            write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
-			write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
+            // If we're not bold write the AND mask.
-			// If we're not bold write the AND mask.
+            //if(!(flags & FONT_BOLD))
-			//if(!(flags & FONT_BOLD))
+            write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
-			write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
+            addr += GRAPHICS_WIDTH_REAL / 8;
-			addr += GRAPHICS_WIDTH_REAL / 8;
+            row++;
-			row++;
+        }
-		}
     }
 }
 
-
-
 /**
  * write_char: Draw a character on the current draw buffer.
  * Currently supports outlined characters and characters with
@@ -1187,109 +1138,109 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
  */
 void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
 {
-    int yy, addr_temp, row, row_temp, xshift;
+    unsigned int yy, addr_temp, row, row_temp, xshift;
     uint16_t and_mask, or_mask, level_bits;
     struct FontEntry font_info;
     char lookup = 0;
     fetch_font_info(ch, font, &font_info, &lookup);
     // Compute starting address (for x,y) of character.
-    int addr = CALC_BUFF_ADDR(x, y);
+    unsigned int addr = CALC_BUFF_ADDR(x, y);
-    int wbit = CALC_BIT_IN_WORD(x);
+    unsigned int wbit = CALC_BIT_IN_WORD(x);
     // If font only supports lowercase or uppercase, make the letter
     // lowercase or uppercase.
     /*if(font_info.flags & FONT_LOWERCASE_ONLY)
-		ch = tolower(ch);
+     ch = tolower(ch);
-    if(font_info.flags & FONT_UPPERCASE_ONLY)
+     if(font_info.flags & FONT_UPPERCASE_ONLY)
-		ch = toupper(ch);*/
+     ch = toupper(ch);*/
     fetch_font_info(ch, font, &font_info, &lookup);
     // How big is the character? We handle characters up to 8 pixels
     // wide for now. Support for large characters may be added in future.
-    if(font_info.width <= 8)
+    if (font_info.width <= 8) {
-    {
+        // Ensure we don't overflow.
-		// Ensure we don't overflow.
+        if (x + wbit > GRAPHICS_WIDTH_REAL) {
-		if(x + wbit > GRAPHICS_WIDTH_REAL)
+            return;
-			return;
+        }
-		// Load data pointer.
+        // Load data pointer.
-		row = lookup * font_info.height * 2;
+        row = lookup * font_info.height * 2;
-		row_temp = row;
+        row_temp = row;
-		addr_temp = addr;
+        addr_temp = addr;
-		xshift = 16 - font_info.width;
+        xshift = 16 - font_info.width;
-		// We can write mask words easily.
+        // We can write mask words easily.
-		for(yy = y; yy < y + font_info.height; yy++)
+        for (yy = y; yy < y + font_info.height; yy++) {
-		{
+            write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit);
-			write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit);
+            addr += GRAPHICS_WIDTH_REAL / 8;
-			addr += GRAPHICS_WIDTH_REAL / 8;
+            row++;
-			row++;
+        }
-		}
+        // Level bits are more complicated. We need to set or clear
-		// Level bits are more complicated. We need to set or clear
+        // level bits, but only where the mask bit is set; otherwise,
-		// level bits, but only where the mask bit is set; otherwise,
+        // we need to leave them alone. To do this, for each word, we
-		// we need to leave them alone. To do this, for each word, we
+        // construct an AND mask and an OR mask, and apply each individually.
-		// construct an AND mask and an OR mask, and apply each individually.
+        row = row_temp;
-		row = row_temp;
+        addr = addr_temp;
-		addr = addr_temp;
+        for (yy = y; yy < y + font_info.height; yy++) {
-		for(yy = y; yy < y + font_info.height; yy++)
+            level_bits = font_info.data[row + font_info.height];
-		{
+            if (!(flags & FONT_INVERT)) {
-			level_bits = font_info.data[row + font_info.height];
+                // data is normally inverted
-			if(!(flags & FONT_INVERT)) // data is normally inverted
+                level_bits = ~level_bits;
-				level_bits = ~level_bits;
+            }
-			or_mask = font_info.data[row] << xshift;
+            or_mask = font_info.data[row] << xshift;
-			and_mask = (font_info.data[row] & level_bits) << xshift;
+            and_mask = (font_info.data[row] & level_bits) << xshift;
-			write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
+            write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
-			// If we're not bold write the AND mask.
+            // If we're not bold write the AND mask.
-			//if(!(flags & FONT_BOLD))
+            //if(!(flags & FONT_BOLD))
-			write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
+            write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit);
-			addr += GRAPHICS_WIDTH_REAL / 8;
+            addr += GRAPHICS_WIDTH_REAL / 8;
-			row++;
+            row++;
-		}
+        }
     }
 }
 
 /**
-* calc_text_dimensions: Calculate the dimensions of a
+ * calc_text_dimensions: Calculate the dimensions of a
-* string in a given font. Supports new lines and
+ * string in a given font. Supports new lines and
-* carriage returns in text.
+ * carriage returns in text.
-*
+ *
-* @param       str                     string to calculate dimensions of
+ * @param       str                     string to calculate dimensions of
-* @param       font_info       font info structure
+ * @param       font_info       font info structure
-* @param       xs                      horizontal spacing
+ * @param       xs                      horizontal spacing
-* @param       ys                      vertical spacing
+ * @param       ys                      vertical spacing
-* @param       dim                     return result: struct FontDimensions
+ * @param       dim                     return result: struct FontDimensions
-*/
+ */
 void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim)
 {
     int max_length = 0, line_length = 0, lines = 1;
-    while(*str != 0)
+    while (*str != 0) {
-    {
+        line_length++;
-		line_length++;
+        if (*str == '\n' || *str == '\r') {
-		if(*str == '\n' || *str == '\r')
+            if (line_length > max_length) {
-		{
+                max_length = line_length;
-			if(line_length > max_length)
+            }
-				max_length = line_length;
+            line_length = 0;
-			line_length = 0;
+            lines++;
-			lines++;
+        }
-		}
+        str++;
-		str++;
+    }
+    if (line_length > max_length) {
+        max_length = line_length;
     }
-    if(line_length > max_length)
-		max_length = line_length;
     dim->width = max_length * (font.width + xs);
     dim->height = lines * (font.height + ys);
 }
 
 /**
-* write_string: Draw a string on the screen with certain
+ * write_string: Draw a string on the screen with certain
-* alignment parameters.
+ * alignment parameters.
-*
+ *
-* @param       str             string to write
+ * @param       str             string to write
-* @param       x               x coordinate
+ * @param       x               x coordinate
-* @param       y               y coordinate
+ * @param       y               y coordinate
-* @param       xs              horizontal spacing
+ * @param       xs              horizontal spacing
-* @param       ys              horizontal spacing
+ * @param       ys              horizontal spacing
-* @param       va              vertical align
+ * @param       va              vertical align
-* @param       ha              horizontal align
+ * @param       ha              horizontal align
-* @param       flags   flags (passed to write_char)
+ * @param       flags   flags (passed to write_char)
-* @param       font    font
+ * @param       font    font
-*/
+ */
 void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font)
 {
     int xx = 0, yy = 0, xx_original = 0;
@@ -1298,56 +1249,63 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un
     // Determine font info and dimensions/position of the string.
     fetch_font_info(0, font, &font_info, NULL);
     calc_text_dimensions(str, font_info, xs, ys, &dim);
-    switch(va)
+    switch (va) {
-    {
+        case TEXT_VA_TOP:
-		case TEXT_VA_TOP:               yy = y; break;
+            yy = y;
-		case TEXT_VA_MIDDLE:    yy = y - (dim.height / 2); break;
+            break;
-		case TEXT_VA_BOTTOM:    yy = y - dim.height; break;
+        case TEXT_VA_MIDDLE:
+            yy = y - (dim.height / 2);
+            break;
+        case TEXT_VA_BOTTOM:
+            yy = y - dim.height;
+            break;
     }
-    switch(ha)
+    switch (ha) {
-    {
+        case TEXT_HA_LEFT:
-		case TEXT_HA_LEFT:              xx = x; break;
+            xx = x;
-		case TEXT_HA_CENTER:    xx = x - (dim.width / 2); break;
+            break;
-		case TEXT_HA_RIGHT:             xx = x - dim.width; break;
+        case TEXT_HA_CENTER:
+            xx = x - (dim.width / 2);
+            break;
+        case TEXT_HA_RIGHT:
+            xx = x - dim.width;
+            break;
     }
     // Then write each character.
     xx_original = xx;
-    while(*str != 0)
+    while (*str != 0) {
-    {
+        if (*str == '\n' || *str == '\r') {
-		if(*str == '\n' || *str == '\r')
+            yy += ys + font_info.height;
-		{
+            xx = xx_original;
-			yy += ys + font_info.height;
+        } else {
-			xx = xx_original;
+            if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) {
-		}
+                if (font_info.id < 2) {
-		else
+                    write_char(*str, xx, yy, flags, font);
-		{
+                } else {
-			if(xx >= 0 && xx < GRAPHICS_WIDTH_REAL)
+                    write_char16(*str, xx, yy, font);
-			{
+                }
-				if(font_info.id<2)
+            }
-					write_char(*str, xx, yy, flags, font);
+            xx += font_info.width + xs;
-				else
+        }
-					write_char16(*str, xx, yy, font);
+        str++;
-			}
-			xx += font_info.width + xs;
-		}
-		str++;
     }
 }
 
 /**
-* write_string_formatted: Draw a string with format escape
+ * write_string_formatted: Draw a string with format escape
-* sequences in it. Allows for complex text effects.
+ * sequences in it. Allows for complex text effects.
-*
+ *
-* @param       str             string to write (with format data)
+ * @param       str             string to write (with format data)
-* @param       x               x coordinate
+ * @param       x               x coordinate
-* @param       y               y coordinate
+ * @param       y               y coordinate
-* @param       xs              default horizontal spacing
+ * @param       xs              default horizontal spacing
-* @param       ys              default horizontal spacing
+ * @param       ys              default horizontal spacing
-* @param       va              vertical align
+ * @param       va              vertical align
-* @param       ha              horizontal align
+ * @param       ha              horizontal align
-* @param       flags   flags (passed to write_char)
+ * @param       flags   flags (passed to write_char)
-*/
+ */
-void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags)
+void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys,
+								__attribute__((unused)) int va, __attribute__((unused)) int ha, int flags)
 {
     int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
     struct FontEntry font_info;
@@ -1363,78 +1321,76 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
     // work out a bounding box. We'll parse again for the final output.
     // This is a simple state machine parser.
     char *ostr = str;
-    while(*str)
+    while (*str) {
-    {
+        if (*str == '<' && fcode == 1) {
-            if(*str == '<' && fcode == 1) // escape code: skip
+            // escape code: skip
-                    fcode = 0;
+            fcode = 0;
-            if(*str == '<' && fcode == 0) // begin format code?
+        }
-            {
+        if (*str == '<' && fcode == 0) {
-                    fcode = 1;
+            // begin format code?
-                    fptr = 0;
+            fcode = 1;
+            fptr = 0;
+        }
+        if (*str == '>' && fcode == 1) {
+            fcode = 0;
+            if (strcmp(fstack, "B")) {
+                // switch to "big" font (font #1)
+                fwidth = bigfontwidth;
+                fheight = bigfontheight;
+            } else if (strcmp(fstack, "S")) {
+                // switch to "small" font (font #0)
+                fwidth = smallfontwidth;
+                fheight = smallfontheight;
             }
-            if(*str == '>' && fcode == 1)
+            if (fheight > max_height) {
-            {
+                max_height = fheight;
-                    fcode = 0;
-                    if(strcmp(fstack, "B")) // switch to "big" font (font #1)
-                    {
-                            fwidth = bigfontwidth;
-                            fheight = bigfontheight;
-                    }
-                    else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
-                    {
-                            fwidth = smallfontwidth;
-                            fheight = smallfontheight;
-                    }
-                    if(fheight > max_height)
-                            max_height = fheight;
-                    // Skip over this byte. Go to next byte.
-                    str++;
-                    continue;
-            }
-            if(*str != '<' && *str != '>' && fcode == 1)
-            {
-                    // Add to the format stack (up to 10 bytes.)
-                    if(fptr > 10) // stop adding bytes
-                    {
-                            str++; // go to next byte
-                            continue;
-                    }
-                    fstack[fptr++] = *str;
-                    fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
-            }
-            if(fcode == 0)
-            {
-                    // Not a format code, raw text.
-                    xx += fwidth + xs;
-                    if(*str == '\n')
-                    {
-                            if(xx > max_xx)
-                                    max_xx = xx;
-                            xx = x;
-                            yy += fheight + ys;
-                    }
             }
+            // Skip over this byte. Go to next byte.
             str++;
+            continue;
+        }
+        if (*str != '<' && *str != '>' && fcode == 1) {
+            // Add to the format stack (up to 10 bytes.)
+            if (fptr > 10) {
+                // stop adding bytes
+                str++; // go to next byte
+                continue;
+            }
+            fstack[fptr++] = *str;
+            fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
+        }
+        if (fcode == 0) {
+            // Not a format code, raw text.
+            xx += fwidth + xs;
+            if (*str == '\n') {
+                if (xx > max_xx) {
+                    max_xx = xx;
+                }
+                xx = x;
+                yy += fheight + ys;
+            }
+        }
+        str++;
     }
     // Reset string pointer.
     str = ostr;
     // Now we've parsed it and got a bbox, we need to work out the dimensions of it
     // and how to align it.
     /*int width = max_xx - x;
-    int height = yy - y;
+     int height = yy - y;
-    int ay, ax;
+     int ay, ax;
-    switch(va)
+     switch(va)
-    {
+     {
-            case TEXT_VA_TOP:               ay = yy; break;
+     case TEXT_VA_TOP:               ay = yy; break;
-            case TEXT_VA_MIDDLE:    ay = yy - (height / 2); break;
+     case TEXT_VA_MIDDLE:    ay = yy - (height / 2); break;
-            case TEXT_VA_BOTTOM:    ay = yy - height; break;
+     case TEXT_VA_BOTTOM:    ay = yy - height; break;
-    }
+     }
-    switch(ha)
+     switch(ha)
-    {
+     {
-            case TEXT_HA_LEFT:              ax = x; break;
+     case TEXT_HA_LEFT:              ax = x; break;
-            case TEXT_HA_CENTER:    ax = x - (width / 2); break;
+     case TEXT_HA_CENTER:    ax = x - (width / 2); break;
-            case TEXT_HA_RIGHT:             ax = x - width; break;
+     case TEXT_HA_RIGHT:             ax = x - width; break;
-    }*/
+     }*/
     // So ax,ay is our new text origin. Parse the text format again and paint
     // the text on the display.
     fcode = 0;
@@ -1442,204 +1398,196 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
     font = 0;
     xx = 0;
     yy = 0;
-    while(*str)
+    while (*str) {
-    {
+        if (*str == '<' && fcode == 1) {
-            if(*str == '<' && fcode == 1) // escape code: skip
+            // escape code: skip
-                    fcode = 0;
+            fcode = 0;
-            if(*str == '<' && fcode == 0) // begin format code?
+        }
-            {
+        if (*str == '<' && fcode == 0) {
-                    fcode = 1;
+            // begin format code?
-                    fptr = 0;
+            fcode = 1;
-            }
+            fptr = 0;
-            if(*str == '>' && fcode == 1)
+        }
-            {
+        if (*str == '>' && fcode == 1) {
-                    fcode = 0;
+            fcode = 0;
-                    if(strcmp(fstack, "B")) // switch to "big" font (font #1)
+            if (strcmp(fstack, "B")) {
-                    {
+                // switch to "big" font (font #1)
-                            fwidth = bigfontwidth;
+                fwidth = bigfontwidth;
-                            fheight = bigfontheight;
+                fheight = bigfontheight;
-                            font = 1;
+                font = 1;
-                    }
+            } else if (strcmp(fstack, "S")) {
-                    else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
+                // switch to "small" font (font #0)
-                    {
+                fwidth = smallfontwidth;
-                            fwidth = smallfontwidth;
+                fheight = smallfontheight;
-                            fheight = smallfontheight;
+                font = 0;
-                            font = 0;
-                    }
-                    // Skip over this byte. Go to next byte.
-                    str++;
-                    continue;
-            }
-            if(*str != '<' && *str != '>' && fcode == 1)
-            {
-                    // Add to the format stack (up to 10 bytes.)
-                    if(fptr > 10) // stop adding bytes
-                    {
-                            str++; // go to next byte
-                            continue;
-                    }
-                    fstack[fptr++] = *str;
-                    fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
-            }
-            if(fcode == 0)
-            {
-                    // Not a format code, raw text. So we draw it.
-                    // TODO - different font sizes.
-                    write_char(*str, xx, yy + (max_height - fheight), flags, font);
-                    xx += fwidth + xs;
-                    if(*str == '\n')
-                    {
-                            if(xx > max_xx)
-                                    max_xx = xx;
-                            xx = x;
-                            yy += fheight + ys;
-                    }
             }
+            // Skip over this byte. Go to next byte.
             str++;
+            continue;
+        }
+        if (*str != '<' && *str != '>' && fcode == 1) {
+            // Add to the format stack (up to 10 bytes.)
+            if (fptr > 10) {
+                // stop adding bytes
+                str++; // go to next byte
+                continue;
+            }
+            fstack[fptr++] = *str;
+            fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
+        }
+        if (fcode == 0) {
+            // Not a format code, raw text. So we draw it.
+            // TODO - different font sizes.
+            write_char(*str, xx, yy + (max_height - fheight), flags, font);
+            xx += fwidth + xs;
+            if (*str == '\n') {
+                if (xx > max_xx) {
+                    max_xx = xx;
+                }
+                xx = x;
+                yy += fheight + ys;
+            }
+        }
+        str++;
     }
 }
 
-
 //SUPEROSD-
 
-
 // graphics
 
 void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t size)
 {
-	int16_t a = mySin(roll+360);
+    int16_t a = mySin(roll + 360);
-	int16_t b = myCos(roll+360);
+    int16_t b = myCos(roll + 360);
-	int16_t c = mySin(roll+90+360)*5/100;
+    int16_t c = mySin(roll + 90 + 360) * 5 / 100;
-	int16_t d = myCos(roll+90+360)*5/100;
+    int16_t d = myCos(roll + 90 + 360) * 5 / 100;
 
-	int16_t k;
+    int16_t k;
-	int16_t l;
+    int16_t l;
 
-	int16_t indi30x1=myCos(30)*(size/2+1) / 100;
+    int16_t indi30x1 = myCos(30) * (size / 2 + 1) / 100;
-	int16_t indi30y1=mySin(30)*(size/2+1) / 100;
+    int16_t indi30y1 = mySin(30) * (size / 2 + 1) / 100;
 
-	int16_t indi30x2=myCos(30)*(size/2+4) / 100;
+    int16_t indi30x2 = myCos(30) * (size / 2 + 4) / 100;
-	int16_t indi30y2=mySin(30)*(size/2+4) / 100;
+    int16_t indi30y2 = mySin(30) * (size / 2 + 4) / 100;
 
-	int16_t indi60x1=myCos(60)*(size/2+1) / 100;
+    int16_t indi60x1 = myCos(60) * (size / 2 + 1) / 100;
-	int16_t indi60y1=mySin(60)*(size/2+1) / 100;
+    int16_t indi60y1 = mySin(60) * (size / 2 + 1) / 100;
 
-	int16_t indi60x2=myCos(60)*(size/2+4) / 100;
+    int16_t indi60x2 = myCos(60) * (size / 2 + 4) / 100;
-	int16_t indi60y2=mySin(60)*(size/2+4) / 100;
+    int16_t indi60y2 = mySin(60) * (size / 2 + 4) / 100;
 
-	pitch=pitch%90;
+    pitch = pitch % 90;
-	if(pitch>90)
+    if (pitch > 90) {
-	{
+        pitch = pitch - 90;
-		pitch=pitch-90;
+    }
-	}
+    if (pitch < -90) {
-	if(pitch<-90)
+        pitch = pitch + 90;
-	{
+    }
-		pitch=pitch+90;
+    a = (a * (size / 2)) / 100;
-	}
+    b = (b * (size / 2)) / 100;
-	a = (a * (size/2)) / 100;
-	b = (b * (size/2)) / 100;
 
-	if(roll<-90 || roll>90)
+    if (roll < -90 || roll > 90) {
-		pitch=pitch*-1;
+        pitch = pitch * -1;
-	k = a*pitch/90;
+    }
-	l = b*pitch/90;
+    k = a * pitch / 90;
+    l = b * pitch / 90;
 
-	// scale
+    // scale
-	//0
+    //0
-	//drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1);
+    //drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1);
-	//drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1);
+    //drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1);
-	write_line_outlined((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1,0,0,0,1);
+    write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
-	write_line_outlined((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1,0,0,0,1);
+    write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
 
-	//30
+    //30
-	//drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2);
+    //drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2);
-	//drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2);
+    //drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2);
-	write_line_outlined((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2,0,0,0,1);
+    write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
-	write_line_outlined((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2,0,0,0,1);
+    write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
-	//60
+    //60
-	//drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2);
+    //drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2);
-	//drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2);
+    //drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2);
-	write_line_outlined((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2,0,0,0,1);
+    write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
-	write_line_outlined((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2,0,0,0,1);
+    write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
-	//90
+    //90
-	//drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1));
+    //drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1));
-	write_line_outlined((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1),0,0,0,1);
+    write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1);
 
+    //roll
+    //drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line
+    write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line
+    //"wingtips"
+    //drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
+    //drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
+    write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1);
+    write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1);
 
-	//roll
+    //pitch
-	//drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line
+    //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
-	write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a,0,0,0,1); //Direction line
+    write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1);
-	//"wingtips"
-	//drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
-	//drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
-	write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c,0,0,0,1);
-	write_line_outlined((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a,0,0,0,1);
 
-	//pitch
+    //drawCircle(x-1, y-1, 5);
-	//drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
+    //write_circle_outlined(x-1, y-1, 5,0,0,0,1);
-	write_line_outlined((x)-1, (y)-1, (x)-1 - k, (y)-1 - l,0,0,0,1);
+    //drawCircle(x-1, y-1, size/2+4);
-
+    //write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1);
-
-	//drawCircle(x-1, y-1, 5);
-	//write_circle_outlined(x-1, y-1, 5,0,0,0,1);
-	//drawCircle(x-1, y-1, size/2+4);
-	//write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1);
 }
 
 void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
 {
-	int i=0;
+    int i = 0;
-	int batteryLines;
+    int batteryLines;
-	//top
+    //top
-	/*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
+    /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
-	drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1);
+     drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1);
 
-	drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2);
+     drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2);
-	//bottom
+     //bottom
-	drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3);
+     drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3);
-	//left
+     //left
-	drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3);
+     drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3);
 
-	//right
+     //right
-	drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/
+     drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/
 
-	write_rectangle_outlined((x)-1, (y)-1+2,size,size*3,0,1);
+    write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1);
-	write_vline_lm((x)-1+(size/2+size/4)+1,(y)-2,(y)-1+1,0,1);
+    write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1);
-	write_vline_lm((x)-1+(size/2-size/4)-1,(y)-2,(y)-1+1,0,1);
+    write_vline_lm((x) - 1 + (size / 2 - size / 4) - 1, (y) - 2, (y) - 1 + 1, 0, 1);
-	write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-2,0,1);
+    write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 2, 0, 1);
-	write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1,1,1);
+    write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1, 1, 1);
-	write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1+1,1,1);
+    write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1 + 1, 1, 1);
 
-	batteryLines = battery*(size*3-2)/100;
+    batteryLines = battery * (size * 3 - 2) / 100;
-	for(i=0;i<batteryLines;i++)
+    for (i = 0; i < batteryLines; i++) {
-	{
+        write_hline_lm((x) - 1, (x) - 1 + size, (y) - 1 + size * 3 - i, 1, 1);
-		write_hline_lm((x)-1,(x)-1 + size,(y)-1+size*3-i,1,1);
+    }
-	}
 }
 
-
+void printTime(uint16_t x, uint16_t y)
-void printTime(uint16_t x, uint16_t y) {
+{
-	char temp[9]={0};
+    char temp[9] =
-	sprintf(temp,"%02d:%02d:%02d",timex.hour,timex.min,timex.sec);
+    { 0 };
-	//printTextFB(x,y,temp);
+    sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec);
-	write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
+    //printTextFB(x,y,temp);
+    write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
 }
 
 /*
-void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
+ void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
 
-	char temp[9]={0};
+ char temp[9]={0};
-	char updown=' ';
+ char updown=' ';
-	uint16_t charx=x/16;
+ uint16_t charx=x/16;
-	if(dir==0)
+ if(dir==0)
-		updown=24;
+ updown=24;
-	if(dir==1)
+ if(dir==1)
-		updown=25;
+ updown=25;
-	sprintf(temp,"%c%6dm",updown,alt);
+ sprintf(temp,"%c%6dm",updown,alt);
-	printTextFB(charx,y+2,temp);
+ printTextFB(charx,y+2,temp);
-	// frame
+ // frame
-	drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
+ drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
-}*/
+ }*/
 
 /**
  * hud_draw_vertical_scale: Draw a vertical scale.
@@ -1658,148 +1606,134 @@ void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
  * @param       max_val                 maximum expected value (used to compute size of arrow ticker)
  * @param       flags                   special flags (see hud.h.)
  */
-void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int boundtick_len, int max_val, int flags)
+void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len,
+        int boundtick_len, __attribute__((unused)) int max_val, int flags)
 {
-        char temp[15];//, temp2[15];
+    char temp[15];	//, temp2[15];
-        struct FontEntry font_info;
+    struct FontEntry font_info;
-        struct FontDimensions dim;
+    struct FontDimensions dim;
-        // Halign should be in a small span.
+    // Halign should be in a small span.
-        //MY_ASSERT(halign >= -1 && halign <= 1);
+    //MY_ASSERT(halign >= -1 && halign <= 1);
-        // Compute the position of the elements.
+    // Compute the position of the elements.
-        int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0;
+    int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0;
-        if(halign == -1)
+    if (halign == -1) {
-        {
+        majtick_start = x;
-                majtick_start = x;
+        majtick_end = x + majtick_len;
-                majtick_end = x + majtick_len;
+        mintick_start = x;
-                mintick_start = x;
+        mintick_end = x + mintick_len;
-                mintick_end = x + mintick_len;
+        boundtick_start = x;
-                boundtick_start = x;
+        boundtick_end = x + boundtick_len;
-                boundtick_end = x + boundtick_len;
+    } else if (halign == +1) {
-        }
+        x = x - GRAPHICS_HDEADBAND;
-        else if(halign == +1)
+        majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
-        {
+        majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
-        		x=x-GRAPHICS_HDEADBAND;
+        mintick_start = GRAPHICS_WIDTH_REAL - x - 1;
-                majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
+        mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1;
-                majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
+        boundtick_start = GRAPHICS_WIDTH_REAL - x - 1;
-                mintick_start = GRAPHICS_WIDTH_REAL - x - 1;
+        boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1;
-                mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1;
+    }
-                boundtick_start = GRAPHICS_WIDTH_REAL - x - 1;
+    // Retrieve width of large font (font #0); from this calculate the x spacing.
-                boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1;
+    fetch_font_info(0, 0, &font_info, NULL);
-        }
+    int arrow_len = (font_info.height / 2) + 1;             // FIXME, font info being loaded correctly??
-        // Retrieve width of large font (font #0); from this calculate the x spacing.
+    int text_x_spacing = arrow_len;
-        fetch_font_info(0, 0, &font_info, NULL);
+    int max_text_y = 0, text_length = 0;
-        int arrow_len = (font_info.height / 2) + 1;             // FIXME, font info being loaded correctly??
+    int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1
-        int text_x_spacing = arrow_len;
+    // For -(range / 2) to +(range / 2), draw the scale.
-        int max_text_y = 0, text_length = 0;
+    int range_2 = range / 2; //, height_2 = height / 2;
-        int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1
+    int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
-        // For -(range / 2) to +(range / 2), draw the scale.
+    // Iterate through each step.
-        int range_2 = range / 2; //, height_2 = height / 2;
+    for (r = -range_2; r <= +range_2; r++) {
-        int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
+        style = 0;
-        // Iterate through each step.
+        rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
-        for(r = -range_2; r <= +range_2; r++)
+        rv = -rr + range_2; // for number display
-        {
+        if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
-                style = 0;
+            rr += majtick_step / 2;
-                rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
+        if (rr % majtick_step == 0)
-                rv = -rr + range_2; // for number display
+            style = 1; // major tick
-                if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
+        else if (rr % mintick_step == 0)
-                        rr += majtick_step / 2;
+            style = 2; // minor tick
-                if(rr % majtick_step == 0)
+        else
-                        style = 1; // major tick
+            style = 0;
-                else if(rr % mintick_step == 0)
+        if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
-                        style = 2; // minor tick
+            continue;
+        if (style) {
+            // Calculate y position.
+            ys = ((long int) (r * height) / (long int) range) + y;
+            //sprintf(temp, "ys=%d", ys);
+            //con_puts(temp, 0);
+            // Depending on style, draw a minor or a major tick.
+            if (style == 1) {
+                write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
+                memset(temp, ' ', 10);
+                //my_itoa(rv, temp);
+                sprintf(temp, "%d", rv);
+                text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
+                if (text_length > max_text_y)
+                    max_text_y = text_length;
+                if (halign == -1)
+                    write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
                 else
-                        style = 0;
+                    write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
-                if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
+            } else if (style == 2)
-                        continue;
+                write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
-                if(style)
-                {
-                        // Calculate y position.
-                        ys = ((long int)(r * height) / (long int)range) + y;
-                        //sprintf(temp, "ys=%d", ys);
-                        //con_puts(temp, 0);
-                        // Depending on style, draw a minor or a major tick.
-                        if(style == 1)
-                        {
-                                write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
-                                memset(temp, ' ', 10);
-                                //my_itoa(rv, temp);
-                                sprintf(temp,"%d",rv);
-                                text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
-                                if(text_length > max_text_y)
-                                        max_text_y = text_length;
-                                if(halign == -1)
-                                        write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
-                                else
-                                        write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
-                        }
-                        else if(style == 2)
-                                write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
-                }
         }
-        // Generate the string for the value, as well as calculating its dimensions.
+    }
-        memset(temp, ' ', 10);
+    // Generate the string for the value, as well as calculating its dimensions.
-        //my_itoa(v, temp);
+    memset(temp, ' ', 10);
-        sprintf(temp,"%d",v);
+    //my_itoa(v, temp);
-        // TODO: add auto-sizing.
+    sprintf(temp, "%d", v);
-        calc_text_dimensions(temp, font_info, 1, 0, &dim);
+    // TODO: add auto-sizing.
-        int xx = 0, i = 0;
+    calc_text_dimensions(temp, font_info, 1, 0, &dim);
-        if(halign == -1)
+    int xx = 0, i = 0;
-                xx = majtick_end + text_x_spacing;
+    if (halign == -1)
-        else
+        xx = majtick_end + text_x_spacing;
-                xx = majtick_end - text_x_spacing;
+    else
-        // Draw an arrow from the number to the point.
+        xx = majtick_end - text_x_spacing;
-        for(i = 0; i < arrow_len; i++)
+    // Draw an arrow from the number to the point.
-        {
+    for (i = 0; i < arrow_len; i++) {
-                if(halign == -1)
+        if (halign == -1) {
-                {
+            write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1);
-                        write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1);
+            write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1);
-                        write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1);
+            write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1);
-                        write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1);
+            write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1);
-                        write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1);
+        } else {
-                }
+            write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1);
-                else
+            write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
-                {
+            write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
-                        write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1);
+            write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1);
-                        write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
-                        write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
-                        write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1);
-                }
-                // FIXME
-                // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
-                // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
         }
-        if(halign == -1)
+        // FIXME
-        {
+        // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
-                write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
+        // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
-                write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1);
+    }
-                write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
+    if (halign == -1) {
-        }
+        write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
-        else
+        write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1);
-        {
+        write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
-                write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
+    } else {
-                write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1);
+        write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
-                write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
+        write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1);
-        }
+        write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
-        // Draw the text.
+    }
-        if(halign == -1)
+    // Draw the text.
-                write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
+    if (halign == -1)
-        else
+        write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
-                write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
+    else
-        // Then, add a slow cut off on the edges, so the text doesn't sharply
+        write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
-        // disappear. We simply clear the areas above and below the ticker, and we
+    // Then, add a slow cut off on the edges, so the text doesn't sharply
-        // use little markers on the edges.
+    // disappear. We simply clear the areas above and below the ticker, and we
-        if(halign == -1)
+    // use little markers on the edges.
-        {
+    if (halign == -1) {
-                write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0);
+        write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
-                write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0);
+                0);
-        }
+        write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
-        else
+                0);
-        {
+    } else {
-                write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
+        write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
-                write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
+        write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
-        }
+    }
-        write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1);
+    write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1);
-        write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1);
+    write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1);
 }
 
 /**
@@ -1816,601 +1750,613 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
  * @param       majtick_len             major tick length
  * @param       flags                   special flags (see hud.h.)
  */
-void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int flags)
+void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, __attribute__((unused)) int flags)
 {
-        v %= 360; // wrap, just in case.
+    v %= 360; // wrap, just in case.
-        struct FontEntry font_info;
+    struct FontEntry font_info;
-        int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0;
+    int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0;
-        char headingstr[4];
+    char headingstr[4];
-        majtick_start = y;
+    majtick_start = y;
-        majtick_end = y - majtick_len;
+    majtick_end = y - majtick_len;
-        mintick_start = y;
+    mintick_start = y;
-        mintick_end = y - mintick_len;
+    mintick_end = y - mintick_len;
-        textoffset = 8;
+    textoffset = 8;
-        int r, style, rr, xs; // rv,
+    int r, style, rr, xs; // rv,
-        int range_2 = range / 2;
+    int range_2 = range / 2;
-        for(r = -range_2; r <= +range_2; r++)
+    for (r = -range_2; r <= +range_2; r++) {
-        {
+        style = 0;
-                style = 0;
+        rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
-                rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
+        //rv = -rr + range_2; // for number display
-                //rv = -rr + range_2; // for number display
+        if (rr % majtick_step == 0)
-                if(rr % majtick_step == 0)
+            style = 1; // major tick
-                        style = 1; // major tick
+        else if (rr % mintick_step == 0)
-                else if(rr % mintick_step == 0)
+            style = 2; // minor tick
-                        style = 2; // minor tick
+        if (style) {
-                if(style)
+            // Calculate x position.
-                {
+            xs = ((long int) (r * width) / (long int) range) + x;
-                        // Calculate x position.
+            // Draw it.
-                        xs = ((long int)(r * width) / (long int)range) + x;
+            if (style == 1) {
-                        // Draw it.
+                write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
-                        if(style == 1)
+                // Draw heading above this tick.
-                        {
+                // If it's not one of north, south, east, west, draw the heading.
-                                write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
+                // Otherwise, draw one of the identifiers.
-                                // Draw heading above this tick.
+                if (rr % 90 != 0) {
-                                // If it's not one of north, south, east, west, draw the heading.
+                    // We abbreviate heading to two digits. This has the side effect of being easy to compute.
-                                // Otherwise, draw one of the identifiers.
+                    headingstr[0] = '0' + (rr / 100);
-                                if(rr % 90 != 0)
+                    headingstr[1] = '0' + ((rr / 10) % 10);
-                                {
+                    headingstr[2] = 0;
-                                        // We abbreviate heading to two digits. This has the side effect of being easy to compute.
+                    headingstr[3] = 0; // nul to terminate
-                                        headingstr[0] = '0' + (rr / 100);
+                } else {
-                                        headingstr[1] = '0' + ((rr / 10) % 10);
+                    switch (rr) {
-                                        headingstr[2] = 0;
+                        case 0:
-                                        headingstr[3] = 0; // nul to terminate
+                            headingstr[0] = 'N';
-                                }
+                            break;
-                                else
+                        case 90:
-                                {
+                            headingstr[0] = 'E';
-                                        switch(rr)
+                            break;
-                                        {
+                        case 180:
-                                                case 0:   headingstr[0] = 'N'; break;
+                            headingstr[0] = 'S';
-                                                case 90:  headingstr[0] = 'E'; break;
+                            break;
-                                                case 180: headingstr[0] = 'S'; break;
+                        case 270:
-                                                case 270: headingstr[0] = 'W'; break;
+                            headingstr[0] = 'W';
-                                        }
+                            break;
-                                        headingstr[1] = 0;
+                    }
-                                        headingstr[2] = 0;
+                    headingstr[1] = 0;
-                                        headingstr[3] = 0;
+                    headingstr[2] = 0;
-                                }
+                    headingstr[3] = 0;
-                                // +1 fudge...!
-                                write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
-                        }
-                        else if(style == 2)
-                                write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
                 }
+                // +1 fudge...!
+                write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
+            } else if (style == 2)
+                write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
         }
-        // Then, draw a rectangle with the present heading in it.
+    }
-        // We want to cover up any other markers on the bottom.
+    // Then, draw a rectangle with the present heading in it.
-        // First compute font size.
+    // We want to cover up any other markers on the bottom.
-        fetch_font_info(0, 3, &font_info, NULL);
+    // First compute font size.
-        int text_width = (font_info.width + 1) * 3;
+    fetch_font_info(0, 3, &font_info, NULL);
-        int rect_width = text_width + 2;
+    int text_width = (font_info.width + 1) * 3;
-        write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
+    int rect_width = text_width + 2;
-        write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
+    write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
-        headingstr[0] = '0' + (v / 100);
+    write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1);
-        headingstr[1] = '0' + ((v / 10) % 10);
+    headingstr[0] = '0' + (v / 100);
-        headingstr[2] = '0' + (v % 10);
+    headingstr[1] = '0' + ((v / 10) % 10);
-        headingstr[3] = 0;
+    headingstr[2] = '0' + (v % 10);
-        write_string(headingstr, x + 1, majtick_start + textoffset+2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
+    headingstr[3] = 0;
+    write_string(headingstr, x + 1, majtick_start + textoffset + 2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
 }
 // CORE draw routines end here
 
-void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size )
+void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size)
 {
-	float alpha;
+    float alpha;
-	uint8_t vertical=0,horizontal=0;
+    uint8_t vertical = 0, horizontal = 0;
-	int16_t x1,x2;
+    int16_t x1, x2;
-	int16_t y1,y2;
+    int16_t y1, y2;
-	int16_t refx,refy;
+    int16_t refx, refy;
-	alpha=DEG2RAD(angle);
+    alpha = DEG2RAD(angle);
-	refx=l_x + size/2;
+    refx = l_x + size / 2;
-	refy=l_y + size/2;
+    refy = l_y + size / 2;
 
-	//
+    //
-	float k=0;
+    float k = 0;
-	float dx = sinf(alpha)*(pitch/90.0f*(size/2));
+    float dx = sinf(alpha) * (pitch / 90.0f * (size / 2));
-	float dy = cosf(alpha)*(pitch/90.0f*(size/2));
+    float dy = cosf(alpha) * (pitch / 90.0f * (size / 2));
-	int16_t x0 = (size/2)-dx;
+    int16_t x0 = (size / 2) - dx;
-	int16_t y0 = (size/2)+dy;
+    int16_t y0 = (size / 2) + dy;
-	// calculate the line function
+    // calculate the line function
-	if((angle < 90.0f) && (angle > -90.0f))
+    if ((angle < 90.0f) && (angle > -90)) {
-	{
 		vertical = 0;
 		if(fabsf(angle) < 1e-5f) {
 			horizontal = 1;
 		} else {
 			k = tanf(alpha);
 		}
-	}
+    } else {
-	else
+        vertical = 1;
-	{
+    }
-		vertical = 1;
-	}
 
-	// crossing point of line
+    // crossing point of line
-	if(!vertical && !horizontal)
+    if (!vertical && !horizontal) {
-	{
+        // y-y0=k(x-x0)
-		// y-y0=k(x-x0)
+        int16_t x = 0;
-		int16_t x=0;
+        int16_t y = k * (x - x0) + y0;
-		int16_t y=k*(x-x0)+y0;
+        // find right crossing point
-		// find right crossing point
+        x1 = x;
-		x1=x;
+        y1 = y;
-		y1=y;
+        if (y < 0) {
-		if(y<0)
+            y1 = 0;
-		{
+            x1 = ((y1 - y0) + k * x0) / k;
-			y1=0;
+        }
-			x1=((y1-y0)+k*x0)/k;
+        if (y > size) {
-		}
+            y1 = size;
-		if(y>size)
+            x1 = ((y1 - y0) + k * x0) / k;
-		{
+        }
-			y1=size;
+        // left crossing point
-			x1=((y1-y0)+k*x0)/k;
+        x = size;
-		}
+        y = k * (x - x0) + y0;
-		// left crossing point
+        x2 = x;
-		x=size;
+        y2 = y;
-		y=k*(x-x0)+y0;
+        if (y < 0) {
-		x2=x;
+            y2 = 0;
-		y2=y;
+            x2 = ((y2 - y0) + k * x0) / k;
-		if(y<0)
+        }
-		{
+        if (y > size) {
-			y2=0;
+            y2 = size;
-			x2=((y2-y0)+k*x0)/k;
+            x2 = ((y2 - y0) + k * x0) / k;
-		}
+        }
-		if(y>size)
+        // move to location
-		{
+        // horizon line
-			y2=size;
+        write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1);
-			x2=((y2-y0)+k*x0)/k;
+        //fill
-		}
+        if (angle <= 0.0f && angle > -90.0f) {
-		// move to location
+            //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-		// horizon line
+            for (int i = y2; i < size; i++) {
-		write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1);
+                x2 = ((i - y0) + k * x0) / k;
-		//fill
+                if (x2 > size) {
-		if(angle <= 0.0f && angle > -90.0f)
+                    x2 = size;
-		{
+                }
-			//write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
+                if (x2 < 0) {
-			for(int i=y2;i<size;i++)
+                    x2 = 0;
-			{
+                }
-				x2=((i-y0)+k*x0)/k;
+                write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1);
-				if(x2>size)
+            }
-					x2=size;
+        } else if (angle < -90.0f) {
-				if(x2<0)
+            //write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-					x2=0;
+            for (int i = 0; i < y2; i++) {
-				write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1);
+                x2 = ((i - y0) + k * x0) / k;
-			}
+                if (x2 > size) {
-		}
+                    x2 = size;
-		else if(angle < -90.0f)
+                }
-		{
+                if (x2 < 0) {
-			//write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
+                    x2 = 0;
-			for(int i=0;i<y2;i++)
+                }
-			{
+                write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1);
-				x2=((i-y0)+k*x0)/k;
+            }
-				if(x2>size)
+        } else if (angle > 0.0f && angle < 90.0f) {
-					x2=size;
+            //write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-				if(x2<0)
+            for (int i = y1; i < size; i++) {
-					x2=0;
+                x2 = ((i - y0) + k * x0) / k;
-				write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1);
+                if (x2 > size) {
-			}
+                    x2 = size;
-		}
+                }
-		else if(angle > 0.0f && angle < 90.0f)
+                if (x2 < 0) {
-		{
+                    x2 = 0;
-			//write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
+                }
-			for(int i=y1;i<size;i++)
+                write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1);
-			{
+            }
-				x2=((i-y0)+k*x0)/k;
+        } else if (angle > 90.0f) {
-				if(x2>size)
+            //write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-					x2=size;
+            for (int i = 0; i < y1; i++) {
-				if(x2<0)
+                x2 = ((i - y0) + k * x0) / k;
-					x2=0;
+                if (x2 > size) {
-				write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1);
+                    x2 = size;
-			}
+                }
-		}
+                if (x2 < 0) {
-		else if(angle > 90.0f)
+                    x2 = 0;
-		{
+                }
-			//write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
+                write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1);
-			for(int i=0;i<y1;i++)
+            }
-			{
+        }
-				x2=((i-y0)+k*x0)/k;
+    } else if (vertical) {
-				if(x2>size)
+        // horizon line
-					x2=size;
+        write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1);
-				if(x2<0)
+        if (angle >= 90.0f) {
-					x2=0;
+            //write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-				write_hline_lm(x2+l_x,0+l_x,i+l_y,1,1);
+            for (int i = 0; i < size; i++) {
-			}
+                write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1);
-		}
+            }
-	}
+        } else {
-	else if(vertical)
+            //write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-	{
+            for (int i = 0; i < size; i++) {
-		// horizon line
+                write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1);
-		write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1);
+            }
-		if(angle >= 90.0f)
+        }
-		{
+    } else if (horizontal) {
-			//write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
+        // horizon line
-			for(int i=0;i<size;i++)
+        write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1);
-			{
+        if (angle < 0) {
-				write_hline_lm(0+l_x,x0+l_x,i+l_y,1,1);
+            //write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-			}
+            for (int i = 0; i < y0; i++) {
-		}
+                write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
-		else
+            }
-		{
+        } else {
-			//write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
+            //write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-			for(int i=0;i<size;i++)
+            for (int i = y0; i < size; i++) {
-			{
+                write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
-				write_hline_lm(size+l_x,x0+l_x,i+l_y,1,1);
+            }
-			}
+        }
-		}
+    }
-	}
-	else if(horizontal)
-	{
-		// horizon line
-		write_hline_outlined(0+l_x,size+l_x,y0+l_y,0,0,0,1);
-		if(angle<0)
-		{
-			//write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-			for(int i=0;i<y0;i++)
-			{
-				write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
-			}
-		}
-		else
-		{
-			//write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-			for(int i=y0;i<size;i++)
-			{
-				write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
-			}
-		}
-	}
 
-	//sides
+    //sides
-	write_line_outlined(l_x,l_y,l_x,l_y+size,0,0,0,1);
+    write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1);
-	write_line_outlined(l_x+size,l_y,l_x+size,l_y+size,0,0,0,1);
+    write_line_outlined(l_x + size, l_y, l_x + size, l_y + size, 0, 0, 0, 1);
-	//plane
+    //plane
-	write_line_outlined(refx-5,refy,refx+6,refy,0,0,0,1);
+    write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1);
-	write_line_outlined(refx,refy,refx,refy-3,0,0,0,1);
+    write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1);
 }
 
-
+void introText()
-void introText(){
+{
-	write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
+    write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT / 2)), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
 }
 
-void introGraphics() {
+void introGraphics()
-	/* logo */
+{
-	int image=0;
+    /* logo */
-	struct splashEntry splash_info;
+    int image = 0;
-	splash_info = splash[image];
+    struct splashEntry splash_info;
+    splash_info = splash[image];
 
-	copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image);
+    copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image);
 
-	/* frame */
+    /* frame */
-	drawBox(APPLY_HDEADBAND(0),APPLY_VDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT-8),APPLY_VDEADBAND(GRAPHICS_BOTTOM));
+    drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT-8), APPLY_VDEADBAND(GRAPHICS_BOTTOM));
 
-	// Must mask out last half-word because SPI keeps clocking it out otherwise
+    // Must mask out last half-word because SPI keeps clocking it out otherwise
-	for (uint32_t i = 0; i < 8; i++) {
+    for (uint32_t i = 0; i < 8; i++) {
-		write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
+        write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
-		write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
+        write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
-	}
+    }
 }
 
 void calcHomeArrow(int16_t m_yaw)
 {
-	HomeLocationData home;
+    HomeLocationData home;
-	HomeLocationGet (&home);
+    HomeLocationGet(&home);
-	GPSPositionData gpsData;
+    GPSPositionData gpsData;
-	GPSPositionGet (&gpsData);
+    GPSPositionGet(&gpsData);
 
-	/** http://www.movable-type.co.uk/scripts/latlong.html **/
+    /** http://www.movable-type.co.uk/scripts/latlong.html **/
     float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g;
     float elevation;
-    float gcsAlt=home.Altitude; // Home MSL altitude
+    float gcsAlt = home.Altitude; // Home MSL altitude
-    float uavAlt=gpsData.Altitude; // UAV MSL altitude
+    float uavAlt = gpsData.Altitude; // UAV MSL altitude
-    float dAlt=uavAlt-gcsAlt; // Altitude difference
+    float dAlt = uavAlt - gcsAlt; // Altitude difference
 
     // Convert to radians
-    lat1 = DEG2RAD(home.Latitude)/10000000.0f; // Home lat
+    lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat
-    lon1 = DEG2RAD(home.Longitude)/10000000.0f; // Home lon
+    lon1 = DEG2RAD(home.Longitude) / 10000000.0f; // Home lon
-    lat2 = DEG2RAD(gpsData.Latitude)/10000000.0f; // UAV lat
+    lat2 = DEG2RAD(gpsData.Latitude) / 10000000.0f; // UAV lat
-    lon2 = DEG2RAD(gpsData.Longitude)/10000000.0f; // UAV lon
+    lon2 = DEG2RAD(gpsData.Longitude) / 10000000.0f; // UAV lon
 
     // Bearing
     /**
-    var y = Math.sin(dLon) * Math.cos(lat2);
+     var y = Math.sin(dLon) * Math.cos(lat2);
-    var x = Math.cos(lat1)*Math.sin(lat2) -
+     var x = Math.cos(lat1)*Math.sin(lat2) -
-            Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
+     Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
-    var brng = Math.atan2(y, x).toDeg();
+     var brng = Math.atan2(y, x).toDeg();
-    **/
+     **/
-    y = sinf(lon2-lon1) * cosf(lat2);
+    y = sinf(lon2 - lon1) * cosf(lat2);
-    x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2-lon1);
+    x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1);
     brng = RAD2DEG(atan2f(y,x));
-    if(brng<0)
+    if (brng < 0) {
-        brng+=360;
+        brng += 360.0f;
+    }
 
     // yaw corrected bearing, needs compass
-    u2g=brng-180-m_yaw;
+    u2g = brng - 180.0f - m_yaw;
-    if(u2g<0)
+    if (u2g < 0) {
-    	u2g+=360;
+        u2g += 360.0f;
+    }
 
     // Haversine formula for distance
     /**
-    var R = 6371; // km
+     var R = 6371; // km
-    var dLat = (lat2-lat1).toRad();
+     var dLat = (lat2-lat1).toRad();
-    var dLon = (lon2-lon1).toRad();
+     var dLon = (lon2-lon1).toRad();
-    var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
+     var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
-            Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) *
+     Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) *
-            Math.sin(dLon/2) * Math.sin(dLon/2);
+     Math.sin(dLon/2) * Math.sin(dLon/2);
-    var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
+     var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
-    var d = R * c;
+     var d = R * c;
-    **/
+     **/
-    a = sinf((lat2-lat1)/2) * sinf((lat2-lat1)/2) +
+    a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2);
-            cosf(lat1) * cosf(lat2) *
+    c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a));
-            sinf((lon2-lon1)/2) * sinf((lon2-lon1)/2);
+    d = 6371.0f * 1000.0f * c;
-    c = 2 * atan2f(sqrtf(a), sqrtf(1-a));
-    d = 6371 * 1000 * c;
 
     // Elevation  v depends servo direction
     if(d > 0.0f)
-        elevation = 90-RAD2DEG(atanf(dAlt/d));
+        elevation = 90.0f - RAD2DEG(atanf(dAlt/d));
     else
-        elevation = 0;
+        elevation = 0.0f;
     //! TODO: sanity check
 
-	char temp[50]={0};
+    char temp[50] =
-	sprintf(temp,"hea:%d",(int)brng);
+    { 0 };
-	write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+    sprintf(temp, "hea:%d", (int) brng);
-	sprintf(temp,"ele:%d",(int)elevation);
+    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-	write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+    sprintf(temp, "ele:%d", (int) elevation);
-	sprintf(temp,"dis:%d",(int)d);
+    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-	write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+    sprintf(temp, "dis:%d", (int) d);
-	sprintf(temp,"u2g:%d",(int)u2g);
+    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-	write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+    sprintf(temp, "u2g:%d", (int) u2g);
+    write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
 
-	sprintf(temp,"%c%c",(int)(u2g/22.5f)*2+0x90,(int)(u2g/22.5f)*2+0x91);
+    sprintf(temp, "%c%c", (int) (u2g / 22.5f) * 2 + 0x90, (int) (u2g / 22.5f) * 2 + 0x91);
-	write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
+    write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
 }
 
-int lama=10;
+int lama = 10;
 int lama_loc[2][30];
 
 void lamas(void)
 {
-	char temp[10]={0};
+    char temp[10] =
-	lama++;
+    { 0 };
-	if(lama%10==0)
+    lama++;
-	{
+    if (lama % 10 == 0) {
-		for(int z=0; z<30;z++)
+        for (int z = 0; z < 30; z++) {
-		{
 
-			lama_loc[0][z]=rand()%(GRAPHICS_RIGHT-10);
+            lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10);
-			lama_loc[1][z]=rand()%(GRAPHICS_BOTTOM-10);
+            lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10);
-		}
+        }
-	}
+    }
-	for(int z=0; z<30;z++)
+    for (int z = 0; z < 30; z++) {
-	{
+        sprintf(temp, "%c", 0xe8 + (lama_loc[0][z] % 2));
-		sprintf(temp,"%c",0xe8+(lama_loc[0][z]%2));
+        write_string(temp, APPLY_HDEADBAND(lama_loc[0][z]), APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-		write_string(temp,APPLY_HDEADBAND(lama_loc[0][z]),APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+    }
-	}
 }
 
 //main draw function
-void updateGraphics() {
+void updateGraphics()
-	OsdSettingsData OsdSettings;
+{
-	OsdSettingsGet (&OsdSettings);
+    OsdSettingsData OsdSettings;
-	AttitudeActualData attitude;
+    OsdSettingsGet(&OsdSettings);
-	AttitudeActualGet(&attitude);
+    AttitudeActualData attitude;
-	GPSPositionData gpsData;
+    AttitudeActualGet(&attitude);
-	GPSPositionGet(&gpsData);
+    GPSPositionData gpsData;
-	HomeLocationData home;
+    GPSPositionGet(&gpsData);
-	HomeLocationGet(&home);
+    HomeLocationData home;
-	BaroAltitudeData baro;
+    HomeLocationGet(&home);
-	BaroAltitudeGet(&baro);
+    BaroAltitudeData baro;
-	
+    BaroAltitudeGet(&baro);
-	switch (OsdSettings.Screen) {
+    FlightStatusData status;
-		case 0: // Dave simple
+    FlightStatusGet(&status);
-		{
-			if(home.Set == HOMELOCATION_SET_FALSE)
-			{
-				char temps[20]={0};
-				sprintf(temps,"HOME NOT SET");
-				//printTextFB(x,y,temp);
-				write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM/2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
-			}
 
-			
+    PIOS_Servo_Set(0, OsdSettings.White);
-			char temp[50]={0};
+    PIOS_Servo_Set(1, OsdSettings.Black);
-			memset(temp, ' ', 40);
-			sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
-			write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
-			sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
-			write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
-			sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
-			write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-			
-			/* Print ADC voltage FLIGHT*/
-			sprintf(temp,"V:%5.2fV",(PIOS_ADC_PinGet(2)*3*6.1f/4096));
-			write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
-			
-			if(gpsData.Heading>180)
-				calcHomeArrow((int16_t)(gpsData.Heading-360));
-			else
-				calcHomeArrow((int16_t)(gpsData.Heading));
-		}
-		break;
-		case 1:
-		{
-				/*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
-				 write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
-				 write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2);
-				 write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/
-				//write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0);
-				//write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0);
-				//drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 );
-				//drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1);
-				//drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2);
-				//drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1);
-				//drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1);
-				/*angleA++;
-				if(angleB<=-90)
-				{
-					sum=2;
-				}
-				if(angleB>=90)
-				{
-					sum=-2;
-				}
-				angleB+=sum;
-				angleC+=2;*/
-				
-				// GPS HACK
-				if(gpsData.Heading>180)
-					calcHomeArrow((int16_t)(gpsData.Heading-360));
-				else
-					calcHomeArrow((int16_t)(gpsData.Heading));
-				
-				/* Draw Attitude Indicator */
-				if(OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED)
-				{
-					drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]),attitude.Pitch,attitude.Roll,96);
-				}
-				//write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
-				//printText16( 60, 12,"Hello OP-OSD");
-				
-				char temp[50]={0};
-				memset(temp, ' ', 40);
-				sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
-				write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-				sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
-				write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-				sprintf(temp,"Fix:%d",(int)gpsData.Status);
-				write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-				sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
-				write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-				
-				
-				/* Print RTC time */
-				if(OsdSettings.Time == OSDSETTINGS_TIME_ENABLED)
-				{
-					printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]),APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
-				}
-				
-				/* Print Number of detected video Lines */
-				sprintf(temp,"Lines:%4d",PIOS_Video_GetOSDLines());
-				write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-				
-				/* Print ADC voltage */
-				//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
-				//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-				sprintf(temp,"Rssi:%4.2fV",(PIOS_ADC_PinGet(4)*3.0f/4096.0f));
-				write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-				
-				/* Print CPU temperature */
-				sprintf(temp,"Temp:%4.2fC",(PIOS_ADC_PinGet(6)*0.29296875f-264));
-				write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-				
-				/* Print ADC voltage FLIGHT*/
-				sprintf(temp,"FltV:%4.2fV",(PIOS_ADC_PinGet(2)*3.0f*6.1f/4096.0f));
-				write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-				
-				/* Print ADC voltage VIDEO*/
-				sprintf(temp,"VidV:%4.2fV",(PIOS_ADC_PinGet(3)*3.0f*6.1f/4096.0f));
-				write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-				
-				/* Print ADC voltage RSSI */
-				//sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
-				//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-				
-				/* Draw Battery Gauge */
-				/*m_batt++;
-				uint8_t dir=3;
-				if(m_batt==101)
-					m_batt=0;
-				if(m_pitch>0)
-				{
-					dir=0;
-					m_alt+=m_pitch/2;
-				}
-				else if(m_pitch<0)
-				{
-					dir=1;
-					m_alt+=m_pitch/2;
-				}*/
 
-				/*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED)
+    switch (OsdSettings.Screen) {
-				{
+        case 0: // Dave simple
-					drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16);
+        {
-				}*/
+            if (home.Set == HOMELOCATION_SET_FALSE) {
-				
+                char temps[20] =
-				//drawAltitude(200,50,m_alt,dir);
+                { 0 };
-				
+                sprintf(temps, "HOME NOT SET");
-				
+                //printTextFB(x,y,temp);
-				//drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
+                write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
-				
+            }
-				// Draw airspeed (left side.)
-				if(OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED)
-				{
-					hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
-							APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
-				}
-				// Draw altimeter (right side.)
-				if(OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED)
-				{
-					hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
-							APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
-				}
-				// Draw compass.
-				if(OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED)
-				{
-					if(attitude.Yaw<0) {
-						hud_draw_linear_compass(360+attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
-								APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
-					} else {
-						hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
-								APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
-					}
-				}
-		}
-		break;
-		case 2:
-		{
-			int size=64;
-			int x=((GRAPHICS_RIGHT/2)-(size/2)),y=(GRAPHICS_BOTTOM-size-2);
-			draw_artificial_horizon(-attitude.Roll,attitude.Pitch,APPLY_HDEADBAND(x),APPLY_VDEADBAND(y),size);
-			hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)),
-					APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
-			if(1)
-			{
-				hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
-					APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
-			}
-			else
-			{
-				hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
-					APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
-			}
 
-		}
+            char temp[50] =
-		break;
+            { 0 };
-		case 3:
+            memset(temp, ' ', 40);
-		{
+            // Note: cast to double required due to -Wdouble-promotion compiler option is
-			lamas();
+            // being used, and there is no way in C to pass a float to a variadic function like sprintf()
-		}
+            sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
-		break;
+            write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
-	default:
+            sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
-		write_vline_lm( APPLY_HDEADBAND(GRAPHICS_RIGHT/2),APPLY_VDEADBAND(0),APPLY_VDEADBAND(GRAPHICS_BOTTOM),1,1);
+            write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
-		write_hline_lm( APPLY_HDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT),APPLY_VDEADBAND(GRAPHICS_BOTTOM/2),1,1);
+            sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
-		break;
+            write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
-	}
+
-	
+            /* Print ADC voltage FLIGHT*/
-	// Must mask out last half-word because SPI keeps clocking it out otherwise
+            sprintf(temp, "V:%5.2fV", (double)(PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096));
-	for (uint32_t i = 0; i < 8; i++) {
+            write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
-		write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
+
-		write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
+            if (gpsData.Heading > 180)
-	}
+                calcHomeArrow((int16_t)(gpsData.Heading - 360));
+            else
+                calcHomeArrow((int16_t)(gpsData.Heading));
+        }
+            break;
+        case 1:
+        {
+            /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4);
+             write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0);
+             write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2);
+             write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/
+            //write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0);
+            //write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0);
+            //drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 );
+            //drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1);
+            //drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2);
+            //drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1);
+            //drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1);
+            /*angleA++;
+             if(angleB<=-90)
+             {
+             sum=2;
+             }
+             if(angleB>=90)
+             {
+             sum=-2;
+             }
+             angleB+=sum;
+             angleC+=2;*/
+
+            // GPS HACK
+            if (gpsData.Heading > 180)
+                calcHomeArrow((int16_t)(gpsData.Heading - 360));
+            else
+                calcHomeArrow((int16_t)(gpsData.Heading));
+
+            /* Draw Attitude Indicator */
+            if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
+                drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),
+                        APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96);
+            }
+            //write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
+            //printText16( 60, 12,"Hello OP-OSD");
+
+            char temp[50] =
+            { 0 };
+            memset(temp, ' ', 40);
+            sprintf(temp, "Lat:%11.7f", (double)(gpsData.Latitude / 10000000.0f));
+            write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+            sprintf(temp, "Lon:%11.7f", (double)(gpsData.Longitude / 10000000.0f));
+            write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+            sprintf(temp, "Fix:%d", (int) gpsData.Status);
+            write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+            sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
+            write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+
+            /* Print RTC time */
+            if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
+                printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
+            }
+
+            /* Print Number of detected video Lines */
+            sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines());
+            write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
+
+            /* Print ADC voltage */
+            //sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
+            //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
+            sprintf(temp, "Rssi:%4.2fV", (double)(PIOS_ADC_PinGet(5) * 3.0f / 4096.0f));
+            write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
+
+            /* Print CPU temperature */
+            sprintf(temp, "Temp:%4.2fC", (double)(PIOS_ADC_PinGet(3) * 0.29296875f - 264));
+            write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
+
+            /* Print ADC voltage FLIGHT*/
+            sprintf(temp, "FltV:%4.2fV", (double)(PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f));
+            write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
+
+            /* Print ADC voltage VIDEO*/
+            sprintf(temp, "VidV:%4.2fV", (double)(PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f));
+            write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
+
+            /* Print ADC voltage RSSI */
+            //sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
+            //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
+            /* Draw Battery Gauge */
+            /*m_batt++;
+             uint8_t dir=3;
+             if(m_batt==101)
+             m_batt=0;
+             if(m_pitch>0)
+             {
+             dir=0;
+             m_alt+=m_pitch/2;
+             }
+             else if(m_pitch<0)
+             {
+             dir=1;
+             m_alt+=m_pitch/2;
+             }*/
+
+            /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED)
+             {
+             drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16);
+             }*/
+
+            //drawAltitude(200,50,m_alt,dir);
+            //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
+            // Draw airspeed (left side.)
+            if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
+                hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
+                        APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
+            }
+            // Draw altimeter (right side.)
+            if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
+                hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
+                        APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
+            }
+            // Draw compass.
+            if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
+                if (attitude.Yaw < 0) {
+                    hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
+                            APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
+                } else {
+                    hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
+                            APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
+                }
+            }
+        }
+            break;
+        case 2:
+        {
+            int size = 64;
+            int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2);
+            draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size);
+            hud_draw_vertical_scale((int) gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7,
+                    10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
+            if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) {
+                hud_draw_vertical_scale((int) baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
+            } else {
+                hud_draw_vertical_scale((int) gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500,
+                        0);
+            }
+
+            char temp[50] =
+            { 0 };
+            memset(temp, ' ', 50);
+            switch (status.FlightMode) {
+                case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
+                    sprintf(temp, "Man");
+                    break;
+                case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
+                    sprintf(temp, "Stab1");
+                    break;
+                case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
+                    sprintf(temp, "Stab2");
+                    break;
+                case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
+                    sprintf(temp, "Stab3");
+                    break;
+                case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+                    sprintf(temp, "PH");
+                    break;
+                case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+                    sprintf(temp, "RTB");
+                    break;
+                case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+                    sprintf(temp, "PATH");
+                    break;
+                default:
+                    sprintf(temp, "Mode: %d", status.FlightMode);
+                    break;
+            }
+            write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
+        }
+            break;
+        case 3:
+        {
+            lamas();
+        }
+            break;
+        case 4:
+        case 5:
+        case 6:
+        {
+            int image = OsdSettings.Screen - 4;
+            struct splashEntry splash_info;
+            splash_info = splash[image];
+
+            copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2), image);
+        }
+            break;
+        default:
+            write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT/2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1);
+            write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2), 1, 1);
+            break;
+    }
+
+    // Must mask out last half-word because SPI keeps clocking it out otherwise
+    for (uint32_t i = 0; i < 8; i++) {
+        write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
+        write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
+    }
 }
 
-void updateOnceEveryFrame() {
+void updateOnceEveryFrame()
-	clearGraphics();
+{
-	updateGraphics();
+    clearGraphics();
+    updateGraphics();
 }
 
-
 // ****************
 /**
  * Initialise the gps module
@@ -2420,12 +2366,14 @@ void updateOnceEveryFrame() {
 
 int32_t osdgenStart(void)
 {
-	// Start gps task
+    // Start gps task
-	vSemaphoreCreateBinary( osdSemaphore);
+    vSemaphoreCreateBinary(osdSemaphore);
-	xTaskCreate(osdgenTask, (signed char *)"OSDGEN", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
+    xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
-	//PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, osdgenTaskHandle);
+    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle);
-
+#ifdef PIOS_INCLUDE_WDG
-	return 0;
+    PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN);
+#endif
+    return 0;
 }
 
 /**
@@ -2435,69 +2383,77 @@ int32_t osdgenStart(void)
  */
 int32_t osdgenInitialize(void)
 {
-	AttitudeActualInitialize();
+    AttitudeActualInitialize();
 #ifdef PIOS_INCLUDE_GPS
-	GPSPositionInitialize();
+    GPSPositionInitialize();
 #if !defined(PIOS_GPS_MINIMAL)
-	GPSTimeInitialize();
+    GPSTimeInitialize();
-	GPSSatellitesInitialize();
+    GPSSatellitesInitialize();
 #endif
 #ifdef PIOS_GPS_SETS_HOMELOCATION
-	HomeLocationInitialize();
+    HomeLocationInitialize();
 #endif
 #endif
-	OsdSettingsInitialize();
+    OsdSettingsInitialize();
-	BaroAltitudeInitialize();
+    BaroAltitudeInitialize();
+    FlightStatusInitialize();
 
-	return 0;
+    return 0;
 }
-MODULE_INITCALL(osdgenInitialize, osdgenStart)
+MODULE_INITCALL( osdgenInitialize, osdgenStart)
 
 // ****************
 /**
  * Main osd task. It does not return.
  */
 
-static void osdgenTask(void *parameters)
+static void osdgenTask(__attribute__((unused)) void *parameters)
 {
-	//portTickType lastSysTime;
+    //portTickType lastSysTime;
-	// Loop forever
+    // Loop forever
-	//lastSysTime = xTaskGetTickCount();
+    //lastSysTime = xTaskGetTickCount();
+    OsdSettingsData OsdSettings;
+    OsdSettingsGet(&OsdSettings);
 
-	// intro
+    PIOS_Servo_Set(0, OsdSettings.White);
-	for(int i=0; i<63; i++)
+    PIOS_Servo_Set(1, OsdSettings.Black);
-	{
-        if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
-        {
-			clearGraphics();
-			introGraphics();
-        }
-	}
-	for(int i=0; i<63; i++)
-	{
-        if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
-        {
-			clearGraphics();
-			introGraphics();
-			introText();
-        }
-	}
 
-	while (1)
+    // intro
-	{
+    for (int i = 0; i < 63; i++) {
-        if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
+        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
-        {
+#ifdef PIOS_INCLUDE_WDG
-			updateOnceEveryFrame();
+            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
+#endif
+            clearGraphics();
+            introGraphics();
         }
-		//xSemaphoreTake(osdSemaphore, portMAX_DELAY);
+    }
-		//vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS);
+    for (int i = 0; i < 63; i++) {
-	}
+        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
+#ifdef PIOS_INCLUDE_WDG
+            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
+#endif
+            clearGraphics();
+            introGraphics();
+            introText();
+        }
+    }
+
+    while (1) {
+        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
+#ifdef PIOS_INCLUDE_WDG
+            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
+#endif
+            updateOnceEveryFrame();
+        }
+        //xSemaphoreTake(osdSemaphore, portMAX_DELAY);
+        //vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS);
+    }
 }
 
-
 // ****************
 
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

flight/modules/Osd/osdinput/inc/osdinput.h

@@ -1,15 +1,38 @@
-/*
+/**
- * OpOsd.h
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup osdinputModule osdinput Module
+ * @brief Process osdinput information
+ * @{
  *
- *  Created on: 2.10.2011
+ * @file       osdinput.h
- *      Author: Samba
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      osdinput module, handles osdinput stream
+ * @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 OPOSD_H_
+#ifndef OSDINPUT_H_
-#define OPOSD_H_
+#define OSDINPUT_H_
 
 #include "openpilot.h"
 
-int32_t OpOsdInitialize(void);
+int32_t osdinputInitialize(void);
 
-#endif /* OPOSD_H_ */
+#endif /* OSDINPUT_H_ */

flight/modules/Osd/osdinput/osdinput.c

@@ -36,261 +36,148 @@
 
 #include "attitudeactual.h"
 #include "taskinfo.h"
+#include "flightstatus.h"
 
 #include "fifo_buffer.h"
 
-
 // ****************
 // Private functions
 
-static void OpOsdTask(void *parameters);
+static void osdinputTask(void *parameters);
 
 // ****************
 // Private constants
 
-#define GPS_TIMEOUT_MS                  500
+#define STACK_SIZE_BYTES            1024
-#define NMEA_MAX_PACKET_LENGTH          33 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
-// same as in COM buffer
-
-
-#ifdef PIOS_GPS_SETS_HOMELOCATION
-// Unfortunately need a good size stack for the WMM calculation
-	#define STACK_SIZE_BYTES            800
-#else
-	#define STACK_SIZE_BYTES            1024
-#endif
-
 #define TASK_PRIORITY                   (tskIDLE_PRIORITY + 4)
-
+#define MAX_PACKET_LENGTH 33
 // ****************
 // Private variables
 
 static uint32_t oposdPort;
 
-static xTaskHandle OpOsdTaskHandle;
+static xTaskHandle osdinputTaskHandle;
 
 static char* oposd_rx_buffer;
 t_fifo_buffer rx;
 
-static uint32_t timeOfLastCommandMs;
+enum osd_pkt_type
-static uint32_t timeOfLastUpdateMs;
+{
-static uint32_t numUpdates;
+    OSD_PKT_TYPE_MISC = 0, OSD_PKT_TYPE_NAV = 1, OSD_PKT_TYPE_MAINT = 2, OSD_PKT_TYPE_ATT = 3, OSD_PKT_TYPE_MODE = 4,
-static uint32_t numChecksumErrors;
+};
-static uint32_t numParsingErrors;
 
 // ****************
 /**
- * Initialise the gps module
+ * Initialise the osdinput module
  * \return -1 if initialisation failed
  * \return 0 on success
  */
 
-int32_t OpOsdStart(void)
+int32_t osdinputStart(void)
 {
-	// Start gps task
+    // Start osdinput task
-	xTaskCreate(OpOsdTask, (signed char *)"OSD", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &OpOsdTaskHandle);
+    xTaskCreate(osdinputTask, (signed char *) "OSDINPUT", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdinputTaskHandle);
-	//PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, OpOsdTaskHandle);
 
-	return 0;
+    return 0;
 }
 /**
  * Initialise the gps module
  * \return -1 if initialisation failed
  * \return 0 on success
  */
-int32_t OpOsdInitialize(void)
+int32_t osdinputInitialize(void)
 {
-	AttitudeActualInitialize();
+    AttitudeActualInitialize();
-	// Initialize quaternion
+    FlightStatusInitialize();
-	AttitudeActualData attitude;
+    // Initialize quaternion
-	AttitudeActualGet(&attitude);
+    AttitudeActualData attitude;
-	attitude.q1 = 1;
+    AttitudeActualGet(&attitude);
-	attitude.q2 = 0;
+    attitude.q1 = 1;
-	attitude.q3 = 0;
+    attitude.q2 = 0;
-	attitude.q4 = 0;
+    attitude.q3 = 0;
-	attitude.Roll = 0;
+    attitude.q4 = 0;
-	attitude.Pitch = 0;
+    attitude.Roll = 0;
-	attitude.Yaw = 0;
+    attitude.Pitch = 0;
-	AttitudeActualSet(&attitude);
+    attitude.Yaw = 0;
+    AttitudeActualSet(&attitude);
 
+    oposdPort = PIOS_COM_OSD;
 
-	// TODO: Get gps settings object
+    oposd_rx_buffer = pvPortMalloc(MAX_PACKET_LENGTH);
-	oposdPort = PIOS_COM_OSD;
+    PIOS_Assert(oposd_rx_buffer);
 
-	oposd_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
+    return 0;
-	PIOS_Assert(oposd_rx_buffer);
-
-	return 0;
 }
-MODULE_INITCALL(OpOsdInitialize, OpOsdStart)
+MODULE_INITCALL( osdinputInitialize, osdinputStart)
 
 // ****************
 /**
- * Main gps task. It does not return.
+ * Main osdinput task. It does not return.
  */
 
-static void OpOsdTask(void *parameters)
+static void osdinputTask(__attribute__((unused)) void *parameters)
 {
-	portTickType xDelay = 100 / portTICK_RATE_MS;
+    portTickType xDelay = 100 / portTICK_RATE_MS;
-	portTickType lastSysTime;
+    portTickType lastSysTime;
-	// Loop forever
+    lastSysTime = xTaskGetTickCount();
-	lastSysTime = xTaskGetTickCount();	//portTickType xDelay = 100 / portTICK_RATE_MS;
-	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
-	//GPSPositionData GpsData;
 
-	//uint8_t rx_count = 0;
+    uint8_t rx_count = 0;
-	//bool start_flag = false;
+    bool start_flag = false;
-	//bool found_cr = false;
+    int32_t osdRxOverflow = 0;
-	//int32_t gpsRxOverflow = 0;
+    uint8_t c = 0xAA;
+    // Loop forever
+    while (1) {
+        // This blocks the task until there is something on the buffer
+        while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) {
 
-	numUpdates = 0;
+            // detect start while acquiring stream
-	numChecksumErrors = 0;
+            if (!start_flag && ((c == 0xCB) || (c == 0x34))) {
-	numParsingErrors = 0;
+                start_flag = true;
+                rx_count = 0;
+            } else if (!start_flag) {
+                continue;
+            }
 
-	timeOfLastUpdateMs = timeNowMs;
+            if (rx_count >= 11) {
-	timeOfLastCommandMs = timeNowMs;
+                // Flush the buffer and note the overflow event.
-	uint8_t rx_count = 0;
+                osdRxOverflow++;
-	bool start_flag = false;
+                start_flag = false;
-	//bool found_cr = false;
+                rx_count = 0;
-	int32_t gpsRxOverflow = 0;
+            } else {
-	uint8_t c=0xAA;
+                oposd_rx_buffer[rx_count] = c;
-	// Loop forever
+                rx_count++;
-	while (1)
+            }
-	{
+            if (rx_count == 11) {
-		/*//DMA_Cmd(DMA1_Stream2, DISABLE);   //prohibit  channel3 for a little time
+                if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) {
-		uint16_t cnt = DMA_GetCurrDataCounter(DMA1_Stream2);
+                    AttitudeActualData attitude;
-    	rx.wr = rx.buf_size-cnt;
+                    AttitudeActualGet(&attitude);
-		if(rx.wr)
+                    attitude.q1 = 1;
-		{
+                    attitude.q2 = 0;
-			//PIOS_LED_Toggle(LED2);
+                    attitude.q3 = 0;
-			while (	fifoBuf_getData(&rx, &c, 1) > 0)
+                    attitude.q4 = 0;
-			{
+                    attitude.Roll = (float) ((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f;
-
+                    attitude.Pitch = (float) ((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f;
-				// detect start while acquiring stream
+                    attitude.Yaw = (float) ((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f;
-				if (!start_flag && ((c == 0xCB) || (c == 0x34)))
+                    AttitudeActualSet(&attitude);
-				{
+                } else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) {
-					start_flag = true;
+                    FlightStatusData status;
-					rx_count = 0;
+                    FlightStatusGet(&status);
-				}
+                    status.Armed = oposd_rx_buffer[8];
-				else
+                    status.FlightMode = oposd_rx_buffer[3];
-				if (!start_flag)
+                    FlightStatusSet(&status);
-					continue;
+                }
-
+                //frame completed
-				if (rx_count >= 11)
+                start_flag = false;
-				{
+                rx_count = 0;
-					// The buffer is already full and we haven't found a valid NMEA sentence.
+            }
-					// Flush the buffer and note the overflow event.
+        }
-					gpsRxOverflow++;
+        vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
-					start_flag = false;
+    }
-					rx_count = 0;
-				}
-				else
-				{
-					oposd_rx_buffer[rx_count] = c;
-					rx_count++;
-				}
-				if (start_flag && rx_count == 11)
-				{
-					//PIOS_LED_Toggle(LED3);
-					if(oposd_rx_buffer[1]==3)
-					{
-						AttitudeActualData attitude;
-						AttitudeActualGet(&attitude);
-						attitude.q1 = 1;
-						attitude.q2 = 0;
-						attitude.q3 = 0;
-						attitude.q4 = 0;
-						attitude.Roll = (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8);
-						attitude.Pitch = (int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8);
-						attitude.Yaw = (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8);
-						AttitudeActualSet(&attitude);
-						//setAttitudeOsd((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8), //pitch
-						//		(int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8), //roll
-						//		(int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8)); //yaw
-
-					}
-					//frame completed
-					start_flag = false;
-					rx_count = 0;
-				}
-			}
-		}
-		//DMA_Cmd(DMA1_Stream2, ENABLE);
-
-		 */
-
-		//PIOS_COM_SendBufferNonBlocking(oposdPort, &c, 1);
-
-		// This blocks the task until there is something on the buffer
-		while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0)
-		{
-
-			// detect start while acquiring stream
-			if (!start_flag && ((c == 0xCB) || (c == 0x34)))
-			{
-				start_flag = true;
-				rx_count = 0;
-			}
-			else
-			if (!start_flag)
-				continue;
-
-			if (rx_count >= 11)
-			{
-				// The buffer is already full and we haven't found a valid NMEA sentence.
-				// Flush the buffer and note the overflow event.
-				gpsRxOverflow++;
-				start_flag = false;
-				rx_count = 0;
-			}
-			else
-			{
-				oposd_rx_buffer[rx_count] = c;
-				rx_count++;
-			}
-			if (rx_count == 11)
-			{
-				if(oposd_rx_buffer[1]==3)
-				{
-					AttitudeActualData attitude;
-					AttitudeActualGet(&attitude);
-					attitude.q1 = 1;
-					attitude.q2 = 0;
-					attitude.q3 = 0;
-					attitude.q4 = 0;
-					attitude.Roll = (int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4]<<8);
-					attitude.Pitch = (int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8);
-					attitude.Yaw = (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8);
-					AttitudeActualSet(&attitude);
-				}
-				//frame completed
-				start_flag = false;
-				rx_count = 0;
-
-			}
-		}
-		vTaskDelayUntil(&lastSysTime, 50 / portTICK_RATE_MS);
-		// Check for GPS timeout
-		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-		if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
-		{	// we have not received any valid GPS sentences for a while.
-			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
-
-
-		}
-		else
-		{	// we appear to be receiving GPS sentences OK, we've had an update
-
-
-		}
-
-	}
 }
 
-
 // ****************
 
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

flight/modules/Osd/osdoutput/inc/osdoutput.h

@@ -0,0 +1,40 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup osdoutputModule osdoutput Module
+ * @brief Process osdoutput information
+ * @{
+ *
+ * @file       osdoutput.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      osdoutput module, handles osdoutput stream
+ * @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 OSDOUTPUT_H
+#define OSDOUTPUT_H
+
+int32_t osdoutputInitialize(void);
+
+#endif	/* OSDOUTPUT_H */
+
+/**
+ * @}
+ * @}
+ */

flight/modules/Osd/osdoutput/osdoutput.c

@@ -0,0 +1,304 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup OSDOUTPUTModule OSDOutput Module
+ * @brief On screen display support
+ * @{ 
+ *
+ * @file       osdoutput.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Interfacing with OSD 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
+ */
+
+#include "openpilot.h"
+
+#if FLIGHTBATTERYSTATE_SUPPORTED
+#include "flightbatterystate.h"
+#endif
+
+#if POSITIONACTUAL_SUPPORTED
+#include "positionactual.h"
+#endif
+
+#include "systemalarms.h"
+#include "attitudeactual.h"
+#include "hwsettings.h"
+#include "flightstatus.h"
+
+static bool osdoutputEnabled;
+
+enum osd_hk_sync
+{
+    OSD_HK_SYNC_A = 0xCB, OSD_HK_SYNC_B = 0x34,
+};
+
+enum osd_hk_pkt_type
+{
+    OSD_HK_PKT_TYPE_MISC = 0, OSD_HK_PKT_TYPE_NAV = 1, OSD_HK_PKT_TYPE_MAINT = 2, OSD_HK_PKT_TYPE_ATT = 3, OSD_HK_PKT_TYPE_MODE = 4,
+};
+
+enum osd_hk_control_mode
+{
+    OSD_HK_CONTROL_MODE_MANUAL = 0, OSD_HK_CONTROL_MODE_STABILIZED = 1, OSD_HK_CONTROL_MODE_AUTO = 2,
+};
+
+struct osd_hk_blob_misc
+{
+    uint8_t type; /* Always OSD_HK_PKT_TYPE_MISC */
+    int16_t roll;
+    int16_t pitch;
+    //uint16_t home;		/* Big Endian */
+    enum osd_hk_control_mode control_mode;
+    uint8_t low_battery;
+    uint16_t current; /* Big Endian */
+}__attribute__((packed));
+
+struct osd_hk_blob_att
+{
+    uint8_t type; /* Always OSD_HK_PKT_TYPE_ATT */
+    int16_t roll;
+    int16_t pitch;
+    int16_t yaw;
+    int16_t speed; /* Big Endian */
+}__attribute__((packed));
+
+struct osd_hk_blob_nav
+{
+    uint8_t type; /* Always OSD_HK_PKT_TYPE_NAV */
+    uint32_t gps_lat; /* Big Endian */
+    uint32_t gps_lon; /* Big Endian */
+}__attribute__((packed));
+
+struct osd_hk_blob_maint
+{
+    uint8_t type; /* Always OSD_HK_PKT_TYPE_MAINT */
+    uint8_t gps_speed;
+    uint16_t gps_alt; /* Big Endian */
+    uint16_t gps_dis; /* Big Endian */
+    uint8_t status;
+    uint8_t config;
+    uint8_t emerg;
+}__attribute__((packed));
+
+struct osd_hk_blob_mode
+{
+    uint8_t type; /* Always OSD_HK_PKT_TYPE_MODE */
+    uint8_t fltmode;
+    uint16_t gps_alt; /* Big Endian */
+    uint16_t gps_dis; /* Big Endian */
+    uint8_t armed;
+    uint8_t config;
+    uint8_t emerg;
+}__attribute__((packed));
+
+union osd_hk_pkt_blobs
+{
+    struct osd_hk_blob_misc misc;
+    struct osd_hk_blob_nav nav;
+    struct osd_hk_blob_maint maint;
+    struct osd_hk_blob_att att;
+    struct osd_hk_blob_mode mode;
+}__attribute__((packed));
+
+struct osd_hk_msg
+{
+    enum osd_hk_sync sync;
+    enum osd_hk_pkt_type t;
+    union osd_hk_pkt_blobs v;
+}__attribute__((packed));
+
+static struct osd_hk_msg osd_hk_msg_buf;
+
+static volatile bool newPositionActualData = false;
+static volatile bool newBattData = false;
+static volatile bool newAttitudeData = false;
+static volatile bool newAlarmData = false;
+
+static uint32_t osd_hk_com_id;
+static uint8_t osd_hk_msg_dropped;
+static uint8_t osd_packet;
+
+static void send_update(__attribute__((unused)) UAVObjEvent * ev)
+{
+    static enum osd_hk_sync sync = OSD_HK_SYNC_A;
+
+    struct osd_hk_msg * msg = &osd_hk_msg_buf;
+    union osd_hk_pkt_blobs * blob = &(osd_hk_msg_buf.v);
+
+    /* Make sure we have a COM port bound */
+    if (!osd_hk_com_id) {
+        return;
+    }
+
+    FlightStatusData flightStatus;
+
+    /*
+     * Set up the message
+     */
+    msg->sync = sync;
+
+    switch (osd_packet) {
+        case OSD_HK_PKT_TYPE_MISC:
+            break;
+        case OSD_HK_PKT_TYPE_NAV:
+            break;
+        case OSD_HK_PKT_TYPE_MAINT:
+            break;
+        case OSD_HK_PKT_TYPE_ATT:
+            msg->t = OSD_HK_PKT_TYPE_ATT;
+            float roll;
+            AttitudeActualRollGet(&roll);
+            blob->att.roll = (int16_t)(roll * 10);
+
+            float pitch;
+            AttitudeActualPitchGet(&pitch);
+            blob->att.pitch = (int16_t)(pitch * 10);
+
+            float yaw;
+            AttitudeActualYawGet(&yaw);
+            blob->att.yaw = (int16_t)(yaw * 10);
+            break;
+        case OSD_HK_PKT_TYPE_MODE:
+            msg->t = OSD_HK_PKT_TYPE_MODE;
+            FlightStatusGet(&flightStatus);
+            blob->mode.fltmode = flightStatus.FlightMode;
+            blob->mode.armed = flightStatus.Armed;
+            break;
+        default:
+            break;
+    }
+
+    /* Field not supported yet */
+//blob->misc.control_mode = 0;
+    /*if (newAlarmData) {
+     SystemAlarmsData alarms;
+     SystemAlarmsGet(&alarms);
+
+     switch (alarms.Alarm[SYSTEMALARMS_ALARM_BATTERY]) {
+     case SYSTEMALARMS_ALARM_UNINITIALISED:
+     case SYSTEMALARMS_ALARM_OK:
+     blob->misc.low_battery = 0;
+     break;
+     case SYSTEMALARMS_ALARM_WARNING:
+     case SYSTEMALARMS_ALARM_ERROR:
+     case SYSTEMALARMS_ALARM_CRITICAL:
+     default:
+     blob->misc.low_battery = 1;
+     break;
+     }
+
+     newAlarmData = false;
+     }*/
+
+#if FLIGHTBATTERYSUPPORTED
+    if (newBattData) {
+        float consumed_energy;
+        FlightBatteryStateConsumedEnergyGet(&consumed_energy);
+
+        uint16_t current = (uint16_t)(consumed_energy * 10);
+
+        /* convert to big endian */
+        blob->misc.current = (
+                (current & 0xFF00 >> 8) |
+                (current & 0x00FF << 8));
+
+        newBattData = false;
+    }
+#else
+//blob->misc.current = 0;
+#endif
+
+#if POSITIONACTUAL_SUPPORTED
+    if (newPositionActualData) {
+        PositionActualData position;
+        PositionActualGet(&position);
+
+        /* compute 3D distance */
+        float d = sqrt(
+                pow(position.North,2) +
+                pow(position.East,2) +
+                pow(position.Down,2));
+        /* convert from cm to dm (10ths of m) */
+        uint16_t home = (uint16_t)(d / 10);
+
+        /* convert to big endian */
+        blob->misc.home = (
+                (home & 0xFF00 >> 8) |
+                (home & 0x00FF << 8));
+
+        newPositionActualData = false;
+    }
+#else
+//blob->misc.home = 0;
+#endif
+
+    if (!PIOS_COM_SendBufferNonBlocking(osd_hk_com_id, (uint8_t *) &osd_hk_msg_buf, sizeof(osd_hk_msg_buf))) {
+        /* Sent a packet, flip to the opposite sync */
+        if (sync == OSD_HK_SYNC_A) {
+            sync = OSD_HK_SYNC_B;
+        } else {
+            sync = OSD_HK_SYNC_A;
+        }
+    } else {
+        /* Failed to send this update */
+        osd_hk_msg_dropped++;
+    }
+    osd_packet++;
+    if (osd_packet > OSD_HK_PKT_TYPE_MODE) {
+        osd_packet = OSD_HK_PKT_TYPE_MISC;
+    }
+}
+
+static UAVObjEvent ev;
+
+static int32_t osdoutputStart(void)
+{
+    if (osdoutputEnabled) {
+        /* Start a periodic timer to kick sending of an update */
+        EventPeriodicCallbackCreate(&ev, send_update, 25 / portTICK_RATE_MS);
+        return 0;
+    }
+    return -1;
+}
+
+static int32_t osdoutputInitialize(void)
+{
+    osd_hk_com_id = PIOS_COM_OSDHK;
+#ifdef MODULE_OSDOUTPUT_BUILTIN
+    osdoutputEnabled = 1;
+#else
+    HwSettingsInitialize();
+    uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+    HwSettingsOptionalModulesGet(optionalModules);
+    if (optionalModules[HWSETTINGS_OPTIONALMODULES_OSDHK] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+        osdoutputEnabled = 1;
+    } else {
+        osdoutputEnabled = 0;
+    }
+#endif
+    return 0;
+}
+MODULE_INITCALL(osdoutputInitialize, osdoutputStart)
+
+/**
+ * @}
+ * @}
+ */

flight/modules/OveroSync/overosync.c

@@ -162,7 +162,7 @@ static void registerObject(UAVObjHandle obj)
  * when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
  * and then take the semaphrore
  */
-static void overoSyncTask(void *parameters)
+static void overoSyncTask(__attribute__((unused)) void *parameters)
 {
 	UAVObjEvent ev;
 

flight/modules/OveroSync/simulated/overosync.c

@@ -161,7 +161,7 @@ static void registerObject(UAVObjHandle obj)
  * when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
  * and then take the semaphrore
  */
-static void overoSyncTask(void *parameters)
+static void overoSyncTask(__attribute__((unused)) void *parameters)
 {
 	UAVObjEvent ev;
 

flight/modules/PathPlanner/pathplanner.c

@@ -116,7 +116,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
 /**
  * Module task
  */
-static void pathPlannerTask(void *parameters)
+static void pathPlannerTask(__attribute__((unused)) void *parameters)
 {
 	// when the active waypoint changes, update pathDesired
 	WaypointConnectCallback(updatePathDesired);
@@ -208,7 +208,7 @@ static void pathPlannerTask(void *parameters)
 }
 
 // callback function when waypoints changed in any way, update pathDesired
-void updatePathDesired(UAVObjEvent * ev) {
+void updatePathDesired(__attribute__((unused)) UAVObjEvent * ev) {
 
 	// only ever touch pathDesired if pathplanner is enabled
 	if (!pathplanner_active) return;
@@ -238,20 +238,25 @@ void updatePathDesired(UAVObjEvent * ev) {
 	pathDesired.UID = waypointActive.Index;
 
 	if(waypointActive.Index == 0) {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
 		// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
 
-		pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
+		/*pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
 		pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
-		pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];
+		pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
+		pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
+		pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
+		pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
 		pathDesired.StartingVelocity = pathDesired.EndingVelocity;
 	} else {
 		// Get previous waypoint as start point
 		WaypointData waypointPrev;
 		WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
 
-		pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
+		pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
-		pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
+		pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
-		pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
+		pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
 		pathDesired.StartingVelocity = waypointPrev.Velocity;
 	}
 	PathDesiredSet(&pathDesired);

flight/modules/RadioComBridge/RadioComBridge.c

@@ -71,8 +71,9 @@ typedef struct {
 
     // The task handles.
     xTaskHandle telemetryTxTaskHandle;
-    xTaskHandle radioRxTaskHandle;
+    xTaskHandle telemetryRxTaskHandle;
     xTaskHandle radioTxTaskHandle;
+    xTaskHandle radioRxTaskHandle;
 
     // The UAVTalk connection on the com side.
     UAVTalkConnection outUAVTalkCon;
@@ -91,6 +92,9 @@ typedef struct {
     // Should we parse UAVTalk?
     bool parseUAVTalk;
 
+    // We can only configure the hardware once.
+    bool configured;
+
     // The current configured uart speed
     OPLinkSettingsComSpeedOptions comSpeed;
 
@@ -100,8 +104,9 @@ typedef struct {
 // Private functions
 
 static void telemetryTxTask(void *parameters);
-static void radioRxTask(void *parameters);
+static void telemetryRxTask(void *parameters);
 static void radioTxTask(void *parameters);
+static void radioRxTask(void *parameters);
 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length);
 static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
 static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte);
@@ -136,59 +141,65 @@ static int32_t RadioComBridgeStart(void)
         bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id);
         if (is_coordinator) {
 
-            // Set the maximum radio RF power.
-            switch (oplinkSettings.MaxRFPower) {
-            case OPLINKSETTINGS_MAXRFPOWER_125:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
-                break;
-            case OPLINKSETTINGS_MAXRFPOWER_16:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
-                break;
-            case OPLINKSETTINGS_MAXRFPOWER_316:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
-                break;
-            case OPLINKSETTINGS_MAXRFPOWER_63:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
-                break;
-            case OPLINKSETTINGS_MAXRFPOWER_126:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
-                break;
-            case OPLINKSETTINGS_MAXRFPOWER_25:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
-                break;
-            case OPLINKSETTINGS_MAXRFPOWER_50:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
-                break;
-            case OPLINKSETTINGS_MAXRFPOWER_100:
-                PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
-                break;
-	    default:
-	        // do nothing
-		break;
-            }
-
 	    // Set the frequency range.
 	    PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing);
-	    
+
             // Reinitilize the modem.
             PIOS_RFM22B_Reinit(pios_rfm22b_id);
+
+            // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port).
+            data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
+                                  (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
+                                  (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
+        }
+
+        // Set the maximum radio RF power.
+        switch (oplinkSettings.MaxRFPower) {
+        case OPLINKSETTINGS_MAXRFPOWER_125:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
+            break;
+        case OPLINKSETTINGS_MAXRFPOWER_16:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
+            break;
+        case OPLINKSETTINGS_MAXRFPOWER_316:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
+            break;
+        case OPLINKSETTINGS_MAXRFPOWER_63:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
+            break;
+        case OPLINKSETTINGS_MAXRFPOWER_126:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
+            break;
+        case OPLINKSETTINGS_MAXRFPOWER_25:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
+            break;
+        case OPLINKSETTINGS_MAXRFPOWER_50:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
+            break;
+        case OPLINKSETTINGS_MAXRFPOWER_100:
+            PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
+            break;
+        default:
+            // do nothing
+            break;
         }
 
 	// Set the initial frequency.
 	PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency);
 
         // Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
-        xTaskCreate(telemetryTxTask, (signed char *)"telemTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle));
+        xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle));
-        xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle));
+        xTaskCreate(telemetryRxTask, (signed char *)"telemetryRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryRxTaskHandle));
         xTaskCreate(radioTxTask, (signed char *)"radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle));
+        xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle));
 
         // Register the watchdog timers.
 #ifdef PIOS_INCLUDE_WDG
-        PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRY);
+        PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYTX);
-        PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX);
+        PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYRX);
         PIOS_WDG_RegisterFlag(PIOS_WDG_RADIOTX);
+        PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX);
 #endif
-
         return 0;
     }
 
@@ -231,7 +242,8 @@ static int32_t RadioComBridgeInitialize(void)
     data->comTxErrors = 0;
     data->comTxRetries = 0;
     data->UAVTalkErrors = 0;
-    data->parseUAVTalk = false;
+    data->parseUAVTalk = true;
+    data->configured = false;
     data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
     PIOS_COM_RADIO = PIOS_COM_RFM22B;
 
@@ -244,14 +256,14 @@ MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
  *
  * @param[in] parameters  The task parameters
  */
-static void telemetryTxTask(void *parameters)
+static void telemetryTxTask(__attribute__((unused)) void *parameters)
 {
     UAVObjEvent ev;
 
     // Loop forever
     while (1) {
 #ifdef PIOS_INCLUDE_WDG
-        PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRY);
+        PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYTX);
 #endif
         // Wait for queue message
         if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
@@ -290,12 +302,40 @@ static void telemetryTxTask(void *parameters)
     }
 }
 
+/**
+ * Radio tx task.  Receive data packets from the com port and send to the radio.
+ *
+ * @param[in] parameters  The task parameters
+ */
+static void radioTxTask(__attribute__((unused)) void *parameters)
+{
+    // Task loop
+    while (1) {
+#ifdef PIOS_INCLUDE_WDG
+        PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX);
+#endif
+
+        // Wait until the com port is available.
+        if (data->parseUAVTalk || !PIOS_COM_TELEMETRY) {
+            vTaskDelay(5);
+            continue;
+        }
+
+        // Read from the com port.
+        uint8_t serial_data[1];
+        uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEMETRY, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
+        if (bytes_to_process > 0) {
+            PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, serial_data, bytes_to_process);
+        }
+    }
+}
+
 /**
  * Radio rx task.  Receive data packets from the radio and pass them on.
  *
  * @param[in] parameters  The task parameters
  */
-static void radioRxTask(void *parameters)
+static void radioRxTask(__attribute__((unused)) void *parameters)
 {
     // Task loop
     while (1) {
@@ -305,27 +345,32 @@ static void radioRxTask(void *parameters)
         uint8_t serial_data[1];
         uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
         if (bytes_to_process > 0) {
-            for (uint8_t i = 0; i < bytes_to_process; i++) {
+            // Either pass the data through the UAVTalk parser, or just send it to the radio (if we're doing raw comms).
-                if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) {
+            if (data->parseUAVTalk) {
-                    data->UAVTalkErrors++;
+                for (uint8_t i = 0; i < bytes_to_process; i++) {
+                    if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) {
+                        data->UAVTalkErrors++;
+                    }
                 }
+            } else if (PIOS_COM_TELEMETRY) {
+                PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
             }
         }
     }
 }
 
 /**
- * Radio rx task.  Receive data from a com port and pass it on to the radio.
+ * Receive telemetry from the USB/COM port.
  *
  * @param[in] parameters  The task parameters
  */
-static void radioTxTask(void *parameters)
+static void telemetryRxTask(__attribute__((unused)) void *parameters)
 {
     // Task loop
     while (1) {
-        uint32_t inputPort = PIOS_COM_TELEMETRY;
+        uint32_t inputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
 #ifdef PIOS_INCLUDE_WDG
-        PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX);
+        PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYRX);
 #endif
 #if defined(PIOS_INCLUDE_USB)
         // Determine output port (USB takes priority over telemetry port)
@@ -356,7 +401,7 @@ static void radioTxTask(void *parameters)
  */
 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
 {
-    uint32_t outputPort = PIOS_COM_TELEMETRY;
+    uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
 #if defined(PIOS_INCLUDE_USB)
     // Determine output port (USB takes priority over telemetry port)
     if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) {
@@ -573,6 +618,11 @@ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port,
 	// Set the frequency range.
 	PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing);
 
+        // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port).
+        data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
+                              (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
+                              (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
+
         // Update the OPLinkSettings object.
         OPLinkSettingsSet(&oplinkSettings);
     }
@@ -590,11 +640,16 @@ static void updateSettings()
     OPLinkSettingsData oplinkSettings;
     OPLinkSettingsGet(&oplinkSettings);
 
+    // We can only configure the hardware once.
+    if (data->configured) {
+        return;
+    }
+    data->configured = true;
+
     // Configure the main port
     bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id);
     switch (oplinkSettings.MainPort) {
     case OPLINKSETTINGS_MAINPORT_TELEMETRY:
-        data->parseUAVTalk = true;
     case OPLINKSETTINGS_MAINPORT_SERIAL:
         /* Configure the main port for uart serial */
         PIOS_InitUartMainPort();
@@ -610,7 +665,6 @@ static void updateSettings()
     // Configure the flexi port
     switch (oplinkSettings.FlexiPort) {
     case OPLINKSETTINGS_FLEXIPORT_TELEMETRY:
-        data->parseUAVTalk = true;
     case OPLINKSETTINGS_FLEXIPORT_SERIAL:
         /* Configure the flexi port as uart serial */
         PIOS_InitUartFlexiPort();

flight/modules/Sensors/sensors.c

@@ -88,6 +88,8 @@ static float mag_bias[3] = {0,0,0};
 static float mag_scale[3] = {0,0,0};
 static float accel_bias[3] = {0,0,0};
 static float accel_scale[3] = {0,0,0};
+static float gyro_staticbias[3] = {0,0,0};
+static float gyro_scale[3] = {0,0,0};
 
 static float R[3][3] = {{0}};
 static int8_t rotate = 0;
@@ -156,7 +158,7 @@ int32_t mag_test;
  */
 
 uint32_t sensor_dt_us;
-static void SensorsTask(void *parameters)
+static void SensorsTask(__attribute__((unused)) void *parameters)
 {
 	portTickType lastSysTime;
 	uint32_t accel_samples = 0;
@@ -358,9 +360,9 @@ static void SensorsTask(void *parameters)
 		float gyros[3] = {(float) gyro_accum[0] / gyro_samples,
 		                  (float) gyro_accum[1] / gyro_samples,
 		                  (float) gyro_accum[2] / gyro_samples};
-		float gyros_out[3] = {gyros[0] * gyro_scaling,
+		float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
-		                      gyros[1] * gyro_scaling,
+		                      gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
-		                      gyros[2] * gyro_scaling};
+		                      gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2]};
 		if (rotate) {
 			rot_mult(R, gyros_out, gyros);
 			gyrosData.x = gyros[0];
@@ -524,7 +526,7 @@ static void magOffsetEstimation(MagnetometerData *mag)
 /**
  * Locally cache some variables from the AtttitudeSettings object
  */
-static void settingsUpdatedCb(UAVObjEvent * objEv) {
+static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent * objEv) {
 	RevoCalibrationGet(&cal);
 	
 	mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
@@ -539,8 +541,12 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
 	accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
 	accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
 	accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
-	// Do not store gyros_bias here as that comes from the state estimator and should be
+	gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
-	// used from GyroBias directly
+	gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
+	gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
+	gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
+	gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
+	gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
 	
 	// Zero out any adaptive tracking
 	MagBiasData magBias;

flight/modules/Sensors/simulated/sensors.c

@@ -145,7 +145,7 @@ MODULE_INITCALL(SensorsInitialize, SensorsStart)
  * Simulated sensor task.  Run a model of the airframe and produce sensor values
  */
 int sensors_count;
-static void SensorsTask(void *parameters)
+static void SensorsTask(__attribute__((unused)) void *parameters)
 {
 	portTickType lastSysTime;
 

flight/modules/Stabilization/inc/relay_tuning.h

@@ -36,4 +36,4 @@
 
 int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
 
-#endif
+#endif

flight/modules/Stabilization/relay_tuning.c

@@ -32,15 +32,11 @@
  */
 
 #include "openpilot.h"
+#include "stabilization.h"
 #include "relaytuning.h"
 #include "relaytuningsettings.h"
 #include "sin_lookup.h"
 
-//! Private variables
-static const int SIN_RESOLUTION = 180;
-
-#define MAX_AXES 3
-
 /**
  * Apply a step function for the stabilization controller and monitor the 
  * result
@@ -108,7 +104,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
 	if(phase >= 360)
 		phase = 0;
 	accum_sin += sin_lookup_deg(phase) * error;
-	accum_cos += sin_lookup_deg(phase + 90) * error;
+	accum_cos += cos_lookup_deg(phase) * error;
 	accumulated ++;
 
 	// Make sure we've had enough time since last transition then check for a change in the output
@@ -148,4 +144,3 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
 	return 0;
 }
 
-

flight/modules/Stabilization/stabilization.c

@@ -55,6 +55,9 @@
 #include "relay_tuning.h"
 #include "virtualflybar.h"
 
+// Includes for various stabilization algorithms
+#include "relay_tuning.h"
+
 // Private constants
 #define MAX_QUEUE_SIZE 1
 
@@ -125,7 +128,6 @@ int32_t StabilizationInitialize()
 #ifdef DIAG_RATEDESIRED
 	RateDesiredInitialize();
 #endif
-
 	// Code required for relay tuning
 	sin_lookup_initalize();
 	RelayTuningSettingsInitialize();
@@ -139,7 +141,7 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
 /**
  * Module task
  */
-static void stabilizationTask(void* parameters)
+static void stabilizationTask(__attribute__((unused)) void* parameters)
 {
 	UAVObjEvent ev;
 	
@@ -213,7 +215,12 @@ static void stabilizationTask(void* parameters)
 		float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
 			stabDesired.Pitch - attitudeActual.Pitch,
 			stabDesired.Yaw - attitudeActual.Yaw};
-		local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
+		// find shortest way
+		float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
+		if(modulo<0)
+			local_error[2] = modulo + 180.0f;
+		else
+			local_error[2] = modulo - 180.0f;
 #endif
 
 		float gyro_filtered[3];
@@ -279,6 +286,7 @@ static void stabilizationTask(void* parameters)
 					stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
 
 					break;
+
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
 				{
 					if (reinit)
@@ -294,6 +302,7 @@ static void stabilizationTask(void* parameters)
 
 					break;
 				}
+
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
 					if (reinit)
 						pids[PID_RATE_ROLL + i].iAccumulator = 0;
@@ -309,7 +318,7 @@ static void stabilizationTask(void* parameters)
 						rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
 					}
 
-					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
+					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]);
 
 					actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
 					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@@ -425,7 +434,7 @@ static float bound(float val, float range)
 	return val;
 }
 
-static void SettingsUpdatedCb(UAVObjEvent * ev)
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent * ev)
 {
 	StabilizationSettingsGet(&settings);
 	

flight/modules/System/systemmod.c

@@ -72,8 +72,8 @@
 
 #ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
 #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998	// calibrated by running tests/test_cpuload.c
-						// must be updated if the FreeRTOS or compiler
+// must be updated if the FreeRTOS or compiler
-						// optimisation options are changed.
+// optimisation options are changed.
 #endif
 
 #if defined(PIOS_SYSTEM_STACK_SIZE)
@@ -114,15 +114,15 @@ static void updateWDGstats();
  */
 int32_t SystemModStart(void)
 {
-	// Initialize vars
+    // Initialize vars
-	stackOverflow = false;
+    stackOverflow = false;
-	mallocFailed = false;
+    mallocFailed = false;
-	// Create system task
+    // Create system task
-	xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
+    xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
-	// Register task
+    // Register task
-	PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
+    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
 
-	return 0;
+    return 0;
 }
 
 /**
@@ -132,61 +132,64 @@ int32_t SystemModStart(void)
 int32_t SystemModInitialize(void)
 {
 
-	// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
+    // Must registers objects here for system thread because ObjectManager started in OpenPilotInit
-	SystemSettingsInitialize();
+    SystemSettingsInitialize();
-	SystemStatsInitialize();
+    SystemStatsInitialize();
-	FlightStatusInitialize();
+    FlightStatusInitialize();
-	ObjectPersistenceInitialize();
+    ObjectPersistenceInitialize();
 #ifdef DIAG_TASKS
-	TaskInfoInitialize();
+    TaskInfoInitialize();
 #endif
 #ifdef DIAG_I2C_WDG_STATS
-	I2CStatsInitialize();
+    I2CStatsInitialize();
-	WatchdogStatusInitialize();
+    WatchdogStatusInitialize();
 #endif
 
-	objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+    objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
-	if (objectPersistenceQueue == NULL)
+    if (objectPersistenceQueue == NULL)
-		return -1;
+        return -1;
 
-	SystemModStart();
+    SystemModStart();
 
-	return 0;
+    return 0;
 }
 
 MODULE_INITCALL(SystemModInitialize, 0)
 /**
  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
  */
-static void systemTask(void *parameters)
+static void systemTask(__attribute__((unused))void *parameters)
 {
+	/* start the delayed callback scheduler */
+	CallbackSchedulerStart();
+
 	/* create all modules thread */
 	MODULE_TASKCREATE_ALL;
 
-	if (mallocFailed) {
+    if (mallocFailed) {
-		/* We failed to malloc during task creation,
+        /* We failed to malloc during task creation,
-		 * system behaviour is undefined.  Reset and let
+         * system behaviour is undefined.  Reset and let
-		 * the BootFault code recover for us.
+         * the BootFault code recover for us.
-		 */
+         */
-		PIOS_SYS_Reset();
+        PIOS_SYS_Reset();
-	}
+    }
 
 #if defined(PIOS_INCLUDE_IAP)
-	/* Record a successful boot */
+    /* Record a successful boot */
-	PIOS_IAP_WriteBootCount(0);
+    PIOS_IAP_WriteBootCount(0);
 #endif
 
-	// Initialize vars
+    // Initialize vars
-	idleCounter = 0;
+    idleCounter = 0;
-	idleCounterClear = 0;
+    idleCounterClear = 0;
 
-	// Listen for SettingPersistance object updates, connect a callback function
+    // Listen for SettingPersistance object updates, connect a callback function
-	ObjectPersistenceConnectQueue(objectPersistenceQueue);
+    ObjectPersistenceConnectQueue(objectPersistenceQueue);
 
 	// Load a copy of HwSetting active at boot time
 	HwSettingsGet(&bootHwSettings);
-	// Whenever the configuration changes, make sure it is safe to fly
+    // Whenever the configuration changes, make sure it is safe to fly
-	HwSettingsConnectCallback(hwSettingsUpdatedCb);
+    HwSettingsConnectCallback(hwSettingsUpdatedCb);
 
 #ifdef DIAG_TASKS
 	TaskInfoData taskInfoData;
@@ -197,11 +200,11 @@ static void systemTask(void *parameters)
 		// Update the system statistics
 		updateStats();
 
-		// Update the system alarms
+        // Update the system alarms
-		updateSystemAlarms();
+        updateSystemAlarms();
 #ifdef DIAG_I2C_WDG_STATS
-		updateI2Cstats();
+        updateI2Cstats();
-		updateWDGstats();
+        updateWDGstats();
 #endif
 
 #ifdef DIAG_TASKS
@@ -210,34 +213,34 @@ static void systemTask(void *parameters)
 		TaskInfoSet(&taskInfoData);
 #endif
 
-		// Flash the heartbeat LED
+        // Flash the heartbeat LED
 #if defined(PIOS_LED_HEARTBEAT)
-		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+        PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
-		DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
+        DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
 #endif	/* PIOS_LED_HEARTBEAT */
 
-		// Turn on the error LED if an alarm is set
+        // Turn on the error LED if an alarm is set
 #if defined (PIOS_LED_ALARM)
-		if (AlarmsHasWarnings()) {
+        if (AlarmsHasWarnings()) {
-			PIOS_LED_On(PIOS_LED_ALARM);
+            PIOS_LED_On(PIOS_LED_ALARM);
-		} else {
+        } else {
-			PIOS_LED_Off(PIOS_LED_ALARM);
+            PIOS_LED_Off(PIOS_LED_ALARM);
-		}
+        }
 #endif	/* PIOS_LED_ALARM */
 
-		FlightStatusData flightStatus;
+        FlightStatusData flightStatus;
-		FlightStatusGet(&flightStatus);
+        FlightStatusGet(&flightStatus);
 
-		UAVObjEvent ev;
+        UAVObjEvent ev;
-		int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
+        int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
-			SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
+        SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
-			SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
+        SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;
 
-		if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
+        if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
-			// If object persistence is updated call the callback
+            // If object persistence is updated call the callback
-			objectUpdatedCb(&ev);
+            objectUpdatedCb(&ev);
-		}
+        }
-	}
+    }
 }
 
 /**
@@ -245,108 +248,101 @@ static void systemTask(void *parameters)
  */
 static void objectUpdatedCb(UAVObjEvent * ev)
 {
-	ObjectPersistenceData objper;
+    ObjectPersistenceData objper;
-	UAVObjHandle obj;
+    UAVObjHandle obj;
 
-	// If the object updated was the ObjectPersistence execute requested action
+    // If the object updated was the ObjectPersistence execute requested action
-	if (ev->obj == ObjectPersistenceHandle()) {
+    if (ev->obj == ObjectPersistenceHandle()) {
-		// Get object data
+        // Get object data
-		ObjectPersistenceGet(&objper);
+        ObjectPersistenceGet(&objper);
 
-		int retval = 1;
+        int retval = 1;
-		FlightStatusData flightStatus;
+        FlightStatusData flightStatus;
-		FlightStatusGet(&flightStatus);
+        FlightStatusGet(&flightStatus);
 
-		// When this is called because of this method don't do anything
+        // When this is called because of this method don't do anything
-		if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR ||
+        if (objper.Operation == OBJECTPERSISTENCE_OPERATION_ERROR || objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
-			objper.Operation == OBJECTPERSISTENCE_OPERATION_COMPLETED) {
+            return;
-			return;
+        }
-		}
 
-		// Execute action if disarmed
+        // Execute action if disarmed
-		if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
+        if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
-			retval = -1;
+            retval = -1;
-		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
+        } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
-			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
+            if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
-				// Get selected object
+                // Get selected object
-				obj = UAVObjGetByID(objper.ObjectID);
+                obj = UAVObjGetByID(objper.ObjectID);
-				if (obj == 0) {
+                if (obj == 0) {
-					return;
+                    return;
-				}
+                }
-				// Load selected instance
+                // Load selected instance
-				retval = UAVObjLoad(obj, objper.InstanceID);
+                retval = UAVObjLoad(obj, objper.InstanceID);
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
+                retval = UAVObjLoadSettings();
-				retval = UAVObjLoadSettings();
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
+                retval = UAVObjLoadMetaobjects();
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
+            }
-				retval = UAVObjLoadMetaobjects();
+        } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
-			}
+            if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
-		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
+                // Get selected object
-			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
+                obj = UAVObjGetByID(objper.ObjectID);
-				// Get selected object
+                if (obj == 0) {
-				obj = UAVObjGetByID(objper.ObjectID);
+                    return;
-				if (obj == 0) {
+                }
-					return;
+                // Save selected instance
-				}
+                retval = UAVObjSave(obj, objper.InstanceID);
-				// Save selected instance
-				retval = UAVObjSave(obj, objper.InstanceID);
 
-				// Not sure why this is needed
+                // Not sure why this is needed
-				vTaskDelay(10);
+                vTaskDelay(10);
 
-				// Verify saving worked
+                // Verify saving worked
-				if (retval == 0)
+                if (retval == 0)
-					retval = UAVObjLoad(obj, objper.InstanceID);
+                    retval = UAVObjLoad(obj, objper.InstanceID);
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
+                retval = UAVObjSaveSettings();
-				retval = UAVObjSaveSettings();
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
+                retval = UAVObjSaveMetaobjects();
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
+            }
-				retval = UAVObjSaveMetaobjects();
+        } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
-			}
+            if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
-		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
+                // Get selected object
-			if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
+                obj = UAVObjGetByID(objper.ObjectID);
-				// Get selected object
+                if (obj == 0) {
-				obj = UAVObjGetByID(objper.ObjectID);
+                    return;
-				if (obj == 0) {
+                }
-					return;
+                // Delete selected instance
-				}
+                retval = UAVObjDelete(obj, objper.InstanceID);
-				// Delete selected instance
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				retval = UAVObjDelete(obj, objper.InstanceID);
+                retval = UAVObjDeleteSettings();
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
+                retval = UAVObjDeleteMetaobjects();
-				retval = UAVObjDeleteSettings();
+            }
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
+        } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				retval = UAVObjDeleteMetaobjects();
-			}
-		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
 #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
-			retval = PIOS_FLASHFS_Format(0);
+            retval = PIOS_FLASHFS_Format(0);
 #else
 			retval = -1;
 #endif
-		}
+        }
-		switch(retval) {
+        switch (retval) {
-			case 0:
+            case 0:
-				objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
+                objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
-				ObjectPersistenceSet(&objper);
+                ObjectPersistenceSet(&objper);
-				break;
+                break;
-			case -1:
+            case -1:
-				objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
+                objper.Operation = OBJECTPERSISTENCE_OPERATION_ERROR;
-				ObjectPersistenceSet(&objper);
+                ObjectPersistenceSet(&objper);
-				break;
+                break;
-			default:
+            default:
-				break;
+                break;
-		}
+        }
-	}
+    }
 }
 
 /**
  * Called whenever hardware settings changed
  */
-static void hwSettingsUpdatedCb(UAVObjEvent * ev)
+static void hwSettingsUpdatedCb(__attribute__((unused))UAVObjEvent * ev)
 {
     HwSettingsData currentHwSettings;
     HwSettingsGet(&currentHwSettings);
@@ -373,76 +369,75 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
  * Called periodically to update the I2C statistics 
  */
 #ifdef DIAG_I2C_WDG_STATS
-static void updateI2Cstats() 
+static void updateI2Cstats()
 {
 #if defined(PIOS_INCLUDE_I2C)
-	I2CStatsData i2cStats;
+    I2CStatsData i2cStats;
-	I2CStatsGet(&i2cStats);
+    I2CStatsGet(&i2cStats);
-	
+
-	struct pios_i2c_fault_history history;
+    struct pios_i2c_fault_history history;
-	PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors);
+    PIOS_I2C_GetDiagnostics(&history, &i2cStats.event_errors);
-	
+
-	for(uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) {
+    for(uint8_t i = 0; (i < I2C_LOG_DEPTH) && (i < I2CSTATS_EVENT_LOG_NUMELEM); i++) {
-		i2cStats.evirq_log[i] = history.evirq[i];
+        i2cStats.evirq_log[i] = history.evirq[i];
-		i2cStats.erirq_log[i] = history.erirq[i];
+        i2cStats.erirq_log[i] = history.erirq[i];
-		i2cStats.event_log[i] = history.event[i];
+        i2cStats.event_log[i] = history.event[i];
-		i2cStats.state_log[i] = history.state[i];		
+        i2cStats.state_log[i] = history.state[i];
-	}
+    }
-	i2cStats.last_error_type = history.type;
+    i2cStats.last_error_type = history.type;
-	I2CStatsSet(&i2cStats);
+    I2CStatsSet(&i2cStats);
 #endif
 }
 
-static void updateWDGstats() 
+static void updateWDGstats()
 {
-	WatchdogStatusData watchdogStatus;
+    WatchdogStatusData watchdogStatus;
-	watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
+    watchdogStatus.BootupFlags = PIOS_WDG_GetBootupFlags();
-	watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
+    watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
-	WatchdogStatusSet(&watchdogStatus);
+    WatchdogStatusSet(&watchdogStatus);
 }
 #endif
 
-
 /**
  * Called periodically to update the system stats
  */
 static uint16_t GetFreeIrqStackSize(void)
 {
-	uint32_t i = 0x200;
+    uint32_t i = 0x200;
 
 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
-extern uint32_t _irq_stack_top;
+    extern uint32_t _irq_stack_top;
-extern uint32_t _irq_stack_end;
+    extern uint32_t _irq_stack_end;
-uint32_t pattern = 0x0000A5A5;
+    uint32_t pattern = 0x0000A5A5;
-uint32_t *ptr = &_irq_stack_end;
+    uint32_t *ptr = &_irq_stack_end;
 
 #if 1 /* the ugly way accurate but takes more time, useful for debugging */
-	uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4;
+    uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4;
 
-	for (i=0; i< stack_size; i++)
+    for (i=0; i< stack_size; i++)
-	{
+    {
-		if (ptr[i] != pattern)
+        if (ptr[i] != pattern)
-		{
+        {
-			i=i*4;
+            i=i*4;
-			break;
+            break;
-		}
+        }
-	}
+    }
 #else /* faster way but not accurate */
-	if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern)
+    if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern)
-	{
+    {
-		i = IRQSTACK_LIMIT_CRITICAL - 1;
+        i = IRQSTACK_LIMIT_CRITICAL - 1;
-	}
+    }
-	else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern)
+    else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern)
-	{
+    {
-		i = IRQSTACK_LIMIT_WARNING - 1;
+        i = IRQSTACK_LIMIT_WARNING - 1;
-	}
+    }
-	else
+    else
-	{
+    {
-		i = IRQSTACK_LIMIT_WARNING;
+        i = IRQSTACK_LIMIT_WARNING;
-	}
+    }
 #endif
 #endif
-	return i;
+    return i;
 }
 
 /**
@@ -450,43 +445,49 @@ uint32_t *ptr = &_irq_stack_end;
  */
 static void updateStats()
 {
-	static portTickType lastTickCount = 0;
+    static portTickType lastTickCount = 0;
-	SystemStatsData stats;
+    SystemStatsData stats;
 
-	// Get stats and update
+    // Get stats and update
-	SystemStatsGet(&stats);
+    SystemStatsGet(&stats);
-	stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
+    stats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
 #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
-	// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
+    // POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
-	stats.HeapRemaining = 10240;
+    stats.HeapRemaining = 10240;
 #else
-	stats.HeapRemaining = xPortGetFreeHeapSize();
+    stats.HeapRemaining = xPortGetFreeHeapSize();
 #endif
 
-	// Get Irq stack status
+    // Get Irq stack status
-	stats.IRQStackRemaining = GetFreeIrqStackSize();
+    stats.IRQStackRemaining = GetFreeIrqStackSize();
 
-	// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
+    // When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
-	if (idleCounterClear) {
+    if (idleCounterClear) {
-		idleCounter = 0;
+        idleCounter = 0;
-	}
+    }
+
+    portTickType now = xTaskGetTickCount();
+    if (now > lastTickCount) {
+        uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS;	// in ms
+        stats.CPULoad = 100 - (uint8_t) roundf(100.0f * ((float) idleCounter / ((float) dT / 1000.0f)) / (float) IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
+    } //else: TickCount has wrapped, do not calc now
+    lastTickCount = now;
+    idleCounterClear = 1;
 
-	portTickType now = xTaskGetTickCount();
-	if (now > lastTickCount) {
-		uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS;	// in ms
-		stats.CPULoad =
-			100 - (uint8_t) roundf(100.0f * ((float)idleCounter / ((float)dT / 1000.0f)) / (float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
-	} //else: TickCount has wrapped, do not calc now
-	lastTickCount = now;
-	idleCounterClear = 1;
-	
 #if defined(PIOS_INCLUDE_ADC) && defined(PIOS_ADC_USE_TEMP_SENSOR)
-	float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((float)((1 << 12) - 1));
+#if defined(STM32F4XX)
-	const float STM32_TEMP_V25 = 1.43f; /* V */
+    float temp_voltage = 3.3f * PIOS_ADC_PinGet(3) / ((1 << 12) - 1);
-	const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */
+    const float STM32_TEMP_V25 = 0.76f; /* V */
-	stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
+    const float STM32_TEMP_AVG_SLOPE = 2.5f; /* mV/C */
+    stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
+#else
+    float temp_voltage = 3.3f * PIOS_ADC_PinGet(0) / ((1 << 12) - 1);
+    const float STM32_TEMP_V25 = 1.43f; /* V */
+    const float STM32_TEMP_AVG_SLOPE = 4.3f; /* mV/C */
+    stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000.0f / STM32_TEMP_AVG_SLOPE + 25.0f;
 #endif
-	SystemStatsSet(&stats);
+#endif
+    SystemStatsSet(&stats);
 }
 
 /**
@@ -494,66 +495,64 @@ static void updateStats()
  */
 static void updateSystemAlarms()
 {
-	SystemStatsData stats;
+    SystemStatsData stats;
-	UAVObjStats objStats;
+    UAVObjStats objStats;
-	EventStats evStats;
+    EventStats evStats;
-	SystemStatsGet(&stats);
+    SystemStatsGet(&stats);
 
-	// Check heap, IRQ stack and malloc failures
+    // Check heap, IRQ stack and malloc failures
-	if ( mallocFailed
+    if (mallocFailed || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
-	     || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
-	     || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
+            || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
 #endif
-	    ) {
+            ) {
-		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
+        AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
-	} else if (
+    } else if ((stats.HeapRemaining < HEAP_LIMIT_WARNING)
-		(stats.HeapRemaining < HEAP_LIMIT_WARNING)
 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
-	     || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
+    || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
 #endif
-	    ) {
+    ) {
-		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
+        AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
-	} else {
+    } else {
-		AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
+        AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
-	}
+    }
 
-	// Check CPU load
+    // Check CPU load
-	if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
+    if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
-		AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
+        AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
-	} else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
+    } else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
-		AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
+        AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
-	} else {
+    } else {
-		AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
+        AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
-	}
+    }
 
-	// Check for stack overflow
+    // Check for stack overflow
-	if (stackOverflow) {
+    if (stackOverflow) {
-		AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
+        AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
-	} else {
+    } else {
-		AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
+        AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
-	}
+    }
+
+    // Check for event errors
+    UAVObjGetStats(&objStats);
+    EventGetStats(&evStats);
+    UAVObjClearStats();
+    EventClearStats();
+    if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
+        AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
+    } else {
+        AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
+    }
+
+    if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
+        SystemStatsData sysStats;
+        SystemStatsGet(&sysStats);
+        sysStats.EventSystemWarningID = evStats.lastErrorID;
+        sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
+        sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;
+        SystemStatsSet(&sysStats);
+    }
 
-	// Check for event errors
-	UAVObjGetStats(&objStats);
-	EventGetStats(&evStats);
-	UAVObjClearStats();
-	EventClearStats();
-	if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0  || evStats.eventErrors > 0) {
-		AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
-	} else {
-		AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
-	}
-	
-	if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {
-		SystemStatsData sysStats;
-		SystemStatsGet(&sysStats);
-		sysStats.EventSystemWarningID = evStats.lastErrorID;
-		sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;
-		sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;
-		SystemStatsSet(&sysStats);
-	}
-		
 }
 
 /**
@@ -561,26 +560,27 @@ static void updateSystemAlarms()
  */
 void vApplicationIdleHook(void)
 {
-	// Called when the scheduler has no tasks to run
+    // Called when the scheduler has no tasks to run
-	if (idleCounterClear == 0) {
+    if (idleCounterClear == 0) {
-		++idleCounter;
+        ++idleCounter;
-	} else {
+    } else {
-		idleCounter = 0;
+        idleCounter = 0;
-		idleCounterClear = 0;
+        idleCounterClear = 0;
-	}
+    }
 }
 
 /**
  * Called by the RTOS when a stack overflow is detected.
  */
 #define DEBUG_STACK_OVERFLOW 0
-void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
+void vApplicationStackOverflowHook(__attribute__((unused))xTaskHandle * pxTask,
+		__attribute__((unused))signed portCHAR * pcTaskName)
 {
-	stackOverflow = true;
+    stackOverflow = true;
 #if DEBUG_STACK_OVERFLOW
-	static volatile bool wait_here = true;
+    static volatile bool wait_here = true;
-	while(wait_here);
+    while(wait_here);
-	wait_here = true;
+    wait_here = true;
 #endif
 }
 
@@ -590,15 +590,15 @@ void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTas
 #define DEBUG_MALLOC_FAILURES 0
 void vApplicationMallocFailedHook(void)
 {
-	mallocFailed = true;
+    mallocFailed = true;
 #if DEBUG_MALLOC_FAILURES
-	static volatile bool wait_here = true;
+    static volatile bool wait_here = true;
-	while(wait_here);
+    while(wait_here);
-	wait_here = true;
+    wait_here = true;
 #endif
 }
 
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

flight/modules/Telemetry/telemetry.c

@@ -307,7 +307,7 @@ static void processObjEvent(UAVObjEvent * ev)
 /**
  * Telemetry transmit task, regular priority
  */
-static void telemetryTxTask(void *parameters)
+static void telemetryTxTask(__attribute__((unused)) void *parameters)
 {
 	UAVObjEvent ev;
 
@@ -325,7 +325,7 @@ static void telemetryTxTask(void *parameters)
  * Telemetry transmit task, high priority
  */
 #if defined(PIOS_TELEM_PRIORITY_QUEUE)
-static void telemetryTxPriTask(void *parameters)
+static void telemetryTxPriTask(__attribute__((unused)) void *parameters)
 {
 	UAVObjEvent ev;
 
@@ -343,7 +343,7 @@ static void telemetryTxPriTask(void *parameters)
 /**
  * Telemetry transmit task. Processes queue events and periodic updates.
  */
-static void telemetryRxTask(void *parameters)
+static void telemetryRxTask(__attribute__((unused)) void *parameters)
 {
 
 	// Task loop

flight/modules/VtolPathFollower/vtolpathfollower.c

@@ -73,6 +73,11 @@
 #include "paths.h"
 #include "CoordinateConversions.h"
 
+#include "cameradesired.h"
+#include "poilearnsettings.h"
+#include "poilocation.h"
+#include "accessorydesired.h"
+
 // Private constants
 #define MAX_QUEUE_SIZE 4
 #define STACK_SIZE_BYTES 1548
@@ -82,19 +87,22 @@
 
 // Private variables
 static xTaskHandle pathfollowerTaskHandle;
-static PathDesiredData pathDesired;
+static PathStatusData pathStatus;
 static VtolPathFollowerSettingsData vtolpathfollowerSettings;
+static float poiRadius;
 
 // Private functions
 static void vtolPathFollowerTask(void *parameters);
 static void SettingsUpdatedCb(UAVObjEvent * ev);
 static void updateNedAccel();
+static void updatePOIBearing();
 static void updatePathVelocity();
 static void updateEndpointVelocity();
 static void updateFixedAttitude(float* attitude);
-static void updateVtolDesiredAttitude();
+static void updateVtolDesiredAttitude(bool yaw_attitude);
 static float bound(float val, float min, float max);
 static bool vtolpathfollower_enabled;
+static void accessoryUpdated(UAVObjEvent* ev);
 
 /**
  * Initialise the module, called on startup
@@ -102,13 +110,13 @@ static bool vtolpathfollower_enabled;
  */
 int32_t VtolPathFollowerStart()
 {
-	if (vtolpathfollower_enabled) {
+    if (vtolpathfollower_enabled) {
-		// Start main task
+        // Start main task
-		xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
+        xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
-		PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
+        PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
-	}
+    }
 
-	return 0;
+    return 0;
 }
 
 /**
@@ -117,25 +125,29 @@ int32_t VtolPathFollowerStart()
  */
 int32_t VtolPathFollowerInitialize()
 {
-	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+    uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
-	
+
-	HwSettingsOptionalModulesGet(optionalModules);
+    HwSettingsOptionalModulesGet(optionalModules);
-	
+
-	if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+    if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
-		VtolPathFollowerSettingsInitialize();
+        VtolPathFollowerSettingsInitialize();
-		NedAccelInitialize();
+        NedAccelInitialize();
-		PathDesiredInitialize();
+        PathDesiredInitialize();
-		PathStatusInitialize();
+        PathStatusInitialize();
-		VelocityDesiredInitialize();
+        VelocityDesiredInitialize();
-		vtolpathfollower_enabled = true;
+        CameraDesiredInitialize();
-	} else {
+        AccessoryDesiredInitialize();
-		vtolpathfollower_enabled = false;
+        PoiLearnSettingsInitialize();
-	}
+        PoiLocationInitialize();
-	
+        vtolpathfollower_enabled = true;
-	return 0;
+    } else {
+        vtolpathfollower_enabled = false;
+    }
+
+    return 0;
 }
 
-MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart)
+MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart)
 
 static float northVelIntegral = 0;
 static float eastVelIntegral = 0;
@@ -149,118 +161,205 @@ static float throttleOffset = 0;
 /**
  * Module thread, should not return.
  */
-static void vtolPathFollowerTask(void *parameters)
+static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
 {
-	SystemSettingsData systemSettings;
+    SystemSettingsData systemSettings;
-	FlightStatusData flightStatus;
+    FlightStatusData flightStatus;
-	PathStatusData pathStatus;
 
-	portTickType lastUpdateTime;
+    portTickType lastUpdateTime;
-	
-	VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
-	PathDesiredConnectCallback(SettingsUpdatedCb);
-	
-	VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
-	PathDesiredGet(&pathDesired);
-	
-	// Main task loop
-	lastUpdateTime = xTaskGetTickCount();
-	while (1) {
 
-		// Conditions when this runs:
+    VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
-		// 1. Must have VTOL type airframe
+    AccessoryDesiredConnectCallback(accessoryUpdated);
-		// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint  OR
-		//    FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
 
-		SystemSettingsGet(&systemSettings);
+    VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
-		if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
-		{
-			AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
-			vTaskDelay(1000);
-			continue;
-		}
 
-		// Continue collecting data if not enough time
+    // Main task loop
-		vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
+    lastUpdateTime = xTaskGetTickCount();
+    while (1) {
 
-		// Convert the accels into the NED frame
+        // Conditions when this runs:
-		updateNedAccel();
+        // 1. Must have VTOL type airframe
-		
+        // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint  OR
-		FlightStatusGet(&flightStatus);
+        //    FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
-		PathStatusGet(&pathStatus);
 
-		// Check the combinations of flightmode and pathdesired mode
+        SystemSettingsGet(&systemSettings);
-		switch(flightStatus.FlightMode) {
+        if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
-			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
-			case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
-				if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
-					updateEndpointVelocity();
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
-					updateVtolDesiredAttitude();
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) {
-					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+            AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
-				} else {
+            vTaskDelay(1000);
-					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+            continue;
-				}
+        }
-				break;
-			case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
-				pathStatus.UID = pathDesired.UID;
-				pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
-				switch(pathDesired.Mode) {
-					// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
-					case PATHDESIRED_MODE_FLYENDPOINT:
-						updateEndpointVelocity();
-						updateVtolDesiredAttitude();
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
-						break;
-					case PATHDESIRED_MODE_FLYVECTOR:
-					case PATHDESIRED_MODE_FLYCIRCLERIGHT:
-					case PATHDESIRED_MODE_FLYCIRCLELEFT:
-						updatePathVelocity();
-						updateVtolDesiredAttitude();
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
-						break;
-					case PATHDESIRED_MODE_FIXEDATTITUDE:
-						updateFixedAttitude(pathDesired.ModeParameters);
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
-						break;
-					case PATHDESIRED_MODE_DISARMALARM:
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
-						break;
-					default:
-						pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
-						break;
-				}
-				break;
-			default:
-				// Be cleaner and get rid of global variables
-				northVelIntegral = 0;
-				eastVelIntegral = 0;
-				downVelIntegral = 0;
-				northPosIntegral = 0;
-				eastPosIntegral = 0;
-				downPosIntegral = 0;
 
-				// Track throttle before engaging this mode.  Cheap system ident
+        // Continue collecting data if not enough time
-				StabilizationDesiredData stabDesired;
+        vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
-				StabilizationDesiredGet(&stabDesired);
-				throttleOffset = stabDesired.Throttle;
 
-				break;
+        // Convert the accels into the NED frame
-		}
+        updateNedAccel();
 
-		AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
+        FlightStatusGet(&flightStatus);
+        PathStatusGet(&pathStatus);
+        PathDesiredData pathDesired;
+        PathDesiredGet(&pathDesired);
 
-	}
+        // Check the combinations of flightmode and pathdesired mode
+        switch (flightStatus.FlightMode) {
+            case FLIGHTSTATUS_FLIGHTMODE_LAND:
+            case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+            case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+                if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+                    updateEndpointVelocity();
+                    updateVtolDesiredAttitude(false);
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+                } else {
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                }
+                break;
+            case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+                pathStatus.UID = pathDesired.UID;
+                pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+                switch (pathDesired.Mode) {
+                    // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
+                    case PATHDESIRED_MODE_FLYENDPOINT:
+                    case PATHDESIRED_MODE_FLYVECTOR:
+                    case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+                    case PATHDESIRED_MODE_FLYCIRCLELEFT:
+                        updatePathVelocity();
+                        updateVtolDesiredAttitude(false);
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+                        break;
+                    case PATHDESIRED_MODE_FIXEDATTITUDE:
+                        updateFixedAttitude(pathDesired.ModeParameters);
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+                        break;
+                    case PATHDESIRED_MODE_DISARMALARM:
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
+                        break;
+                    default:
+                        pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                        break;
+                }
+                PathStatusSet(&pathStatus);
+                break;
+            case FLIGHTSTATUS_FLIGHTMODE_POI:
+                if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+                    updateEndpointVelocity();
+                    updateVtolDesiredAttitude(true);
+                    updatePOIBearing();
+                } else {
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+                }
+                break;
+            default:
+                // Be cleaner and get rid of global variables
+                northVelIntegral = 0;
+                eastVelIntegral = 0;
+                downVelIntegral = 0;
+                northPosIntegral = 0;
+                eastPosIntegral = 0;
+                downPosIntegral = 0;
+
+                // Track throttle before engaging this mode.  Cheap system ident
+                StabilizationDesiredData stabDesired;
+                StabilizationDesiredGet(&stabDesired);
+                throttleOffset = stabDesired.Throttle;
+
+                break;
+        }
+
+        AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
+
+    }
+}
+
+/**
+ * Compute bearing and elevation between current position and POI
+ */
+static void updatePOIBearing()
+{
+    const float DEADBAND_HIGH = 0.10f;
+    const float DEADBAND_LOW = -0.10f;
+    float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
+
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+    PositionActualData positionActual;
+    PositionActualGet(&positionActual);
+    CameraDesiredData cameraDesired;
+    CameraDesiredGet(&cameraDesired);
+    StabilizationDesiredData stabDesired;
+    StabilizationDesiredGet(&stabDesired);
+    PoiLocationData poi;
+    PoiLocationGet(&poi);
+
+    float dLoc[3];
+    float yaw = 0;
+    /*float elevation = 0;*/
+
+    dLoc[0] = positionActual.North - poi.North;
+    dLoc[1] = positionActual.East - poi.East;
+    dLoc[2] = positionActual.Down - poi.Down;
+
+    if (dLoc[1] < 0) {
+        yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180.0f;
+    } else {
+        yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180.0f;
+    }
+
+    // distance
+    float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
+
+    ManualControlCommandData manualControlData;
+    ManualControlCommandGet(&manualControlData);
+
+    float changeRadius = 0;
+    // Move closer or further, radially
+    if (manualControlData.Pitch > DEADBAND_HIGH) {
+        changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
+    } else if (manualControlData.Pitch < DEADBAND_LOW) {
+        changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
+    }
+
+    // move along circular path
+    float pathAngle = 0;
+    if (manualControlData.Roll > DEADBAND_HIGH) {
+        pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
+    } else if (manualControlData.Roll < DEADBAND_LOW) {
+        pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
+    } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
+        // change radius only when not circling
+        poiRadius = distance + changeRadius;
+    }
+
+    // don't try to move any closer
+    if (poiRadius >= 3.0f || changeRadius > 0) {
+        if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
+            pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
+            pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
+            pathDesired.StartingVelocity = 1.0f;
+            pathDesired.EndingVelocity = 0.0f;
+            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+            PathDesiredSet(&pathDesired);
+        }
+    }
+    //not above
+    if (distance >= 3.0f) {
+        //You can feed this into camerastabilization
+        /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
+
+        stabDesired.Yaw = yaw + (pathAngle / 2.0f);
+        stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+
+        //cameraDesired.Yaw=yaw;
+        //cameraDesired.PitchOrServo2=elevation;
+
+        CameraDesiredSet(&cameraDesired);
+        StabilizationDesiredSet(&stabDesired);
+    }
 }
 
 /**
@@ -271,51 +370,74 @@ static void vtolPathFollowerTask(void *parameters)
  */
 static void updatePathVelocity()
 {
-	float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
+    float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
-	float downCommand;
+    float downCommand;
 
-	PositionActualData positionActual;
+    PathDesiredData pathDesired;
-	PositionActualGet(&positionActual);
+    PathDesiredGet(&pathDesired);
-	
+    PositionActualData positionActual;
-	float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
+    PositionActualGet(&positionActual);
-	struct path_status progress;
-	
-	path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
-	
-	float groundspeed = pathDesired.StartingVelocity + 
-	    (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1);
-	if(progress.fractional_progress > 1)
-		groundspeed = 0;
-	
-	VelocityDesiredData velocityDesired;
-	velocityDesired.North = progress.path_direction[0] * groundspeed;
-	velocityDesired.East = progress.path_direction[1] * groundspeed;
-	
-	float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
-	float correction_velocity[2] = {progress.correction_direction[0] * error_speed, 
-	    progress.correction_direction[1] * error_speed};
-	
-	float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
-	float scale = 1;
-	if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
-		scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
 
-	velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
+    float cur[3] =
-	velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
+    { positionActual.North, positionActual.East, positionActual.Down };
-	
+    struct path_status progress;
-	float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
-	    bound(progress.fractional_progress,0,1);
 
-	float downError = altitudeSetpoint - positionActual.Down;
+    path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
-	downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-							-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
-							vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
-	downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
-	velocityDesired.Down = bound(downCommand,
-								 -vtolpathfollowerSettings.VerticalVelMax,
-								 vtolpathfollowerSettings.VerticalVelMax);
 
-	VelocityDesiredSet(&velocityDesired);
+    float groundspeed;
+    switch (pathDesired.Mode) {
+        case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+        case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+        case PATHDESIRED_MODE_FLYCIRCLELEFT:
+        case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+            groundspeed = pathDesired.EndingVelocity;
+            break;
+        case PATHDESIRED_MODE_FLYENDPOINT:
+        case PATHDESIRED_MODE_DRIVEENDPOINT:
+            groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
+            if (progress.fractional_progress > 1)
+                groundspeed = 0;
+            break;
+        case PATHDESIRED_MODE_FLYVECTOR:
+        case PATHDESIRED_MODE_DRIVEVECTOR:
+        default:
+            groundspeed = pathDesired.StartingVelocity
+                    + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
+            if (progress.fractional_progress > 1)
+                groundspeed = 0;
+            break;
+    }
+
+    VelocityDesiredData velocityDesired;
+    velocityDesired.North = progress.path_direction[0] * groundspeed;
+    velocityDesired.East = progress.path_direction[1] * groundspeed;
+
+    float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
+    float correction_velocity[2] =
+    { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
+
+    float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
+    float scale = 1;
+    if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
+        scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
+
+    velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
+    velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
+
+    float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1);
+
+    float downError = altitudeSetpoint - positionActual.Down;
+    downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
+            -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
+            vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
+    downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
+    velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
+
+    // update pathstatus
+    pathStatus.error = progress.error;
+    pathStatus.fractional_progress = progress.fractional_progress;
+
+    VelocityDesiredSet(&velocityDesired);
 }
 
 /**
@@ -326,38 +448,41 @@ static void updatePathVelocity()
  */
 void updateEndpointVelocity()
 {
-	float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
+    float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
 
-	PositionActualData positionActual;
+    PathDesiredData pathDesired;
-	VelocityDesiredData velocityDesired;
+    PathDesiredGet(&pathDesired);
-	
+
-	PositionActualGet(&positionActual);
+    PositionActualData positionActual;
-	VelocityDesiredGet(&velocityDesired);
+    VelocityDesiredData velocityDesired;
-	
+
-	float northError;
+    PositionActualGet(&positionActual);
-	float eastError;
+    VelocityDesiredGet(&velocityDesired);
-	float downError;
+
-	float northCommand;
+    float northError;
-	float eastCommand;
+    float eastError;
-	float downCommand;
+    float downError;
-	
+    float northCommand;
-	float northPos = 0, eastPos = 0, downPos = 0;
+    float eastCommand;
-	switch (vtolpathfollowerSettings.PositionSource) {
+    float downCommand;
-		case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
+
-			northPos = positionActual.North;
+    float northPos = 0, eastPos = 0, downPos = 0;
-			eastPos = positionActual.East;
+    switch (vtolpathfollowerSettings.PositionSource) {
-			downPos = positionActual.Down;
+        case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
-			break;
+            northPos = positionActual.North;
-		case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
+            eastPos = positionActual.East;
-		{
+            downPos = positionActual.Down;
-			// this used to work with the NEDposition UAVObject
+            break;
-			// however this UAVObject has been removed
+        case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
-			GPSPositionData gpsPosition;
+        {
-			GPSPositionGet(&gpsPosition);
+            // this used to work with the NEDposition UAVObject
-			HomeLocationData homeLocation;
+            // however this UAVObject has been removed
-			HomeLocationGet(&homeLocation);
+            GPSPositionData gpsPosition;
+            GPSPositionGet(&gpsPosition);
+            HomeLocationData homeLocation;
+            HomeLocationGet(&homeLocation);
 			float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
-			float alt = homeLocation.Altitude;
+            float alt = homeLocation.Altitude;
 			float T[3] = { alt+6.378137E6f,
 				     cosf(lat)*(alt+6.378137E6f),
 				     -1.0f};
@@ -365,50 +490,46 @@ void updateEndpointVelocity()
 				T[1] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)),
 				T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))};
 
-			northPos = NED[0];
+            northPos = NED[0];
-			eastPos = NED[1];
+            eastPos = NED[1];
-			downPos = NED[2];
+            downPos = NED[2];
-		}
+        }
-			break;
+            break;
-		default:
+        default:
-			PIOS_Assert(0);
+            PIOS_Assert(0);
-			break;
+            break;
-	}
+    }
 
-	// Compute desired north command
+    // Compute desired north command
-	northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
+    northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
-	northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 
+    northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-			      -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
+            -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
-			      vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
+            vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
-	northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
+    northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral);
-			northPosIntegral);
-	
-	eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
-	eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 
-				 -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
-				 vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
-	eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
-		       eastPosIntegral);
-	
-	// Limit the maximum velocity
-	float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
-	float scale = 1;
-	if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
-		scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
 
-	velocityDesired.North = northCommand * scale;
+    eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
-	velocityDesired.East = eastCommand * scale;
+    eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
+            -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
+            vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
+    eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
 
-	downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
+    // Limit the maximum velocity
-	downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], 
+    float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
-				-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
+    float scale = 1;
-				vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
+    if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
-	downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
+        scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
-	velocityDesired.Down = bound(downCommand,
+
-				     -vtolpathfollowerSettings.VerticalVelMax, 
+    velocityDesired.North = northCommand * scale;
-				     vtolpathfollowerSettings.VerticalVelMax);
+    velocityDesired.East = eastCommand * scale;
-	
+
-	VelocityDesiredSet(&velocityDesired);	
+    downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
+    downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
+            -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
+            vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
+    downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
+    velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
+
+    VelocityDesiredSet(&velocityDesired);
 }
 
 /**
@@ -417,16 +538,16 @@ void updateEndpointVelocity()
  */
 static void updateFixedAttitude(float* attitude)
 {
-	StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-	StabilizationDesiredGet(&stabDesired);
+    StabilizationDesiredGet(&stabDesired);
-	stabDesired.Roll     = attitude[0];
+    stabDesired.Roll = attitude[0];
-	stabDesired.Pitch    = attitude[1];
+    stabDesired.Pitch = attitude[1];
-	stabDesired.Yaw      = attitude[2];
+    stabDesired.Yaw = attitude[2];
-	stabDesired.Throttle = attitude[3];
+    stabDesired.Throttle = attitude[3];
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+    stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-	StabilizationDesiredSet(&stabDesired);
+    StabilizationDesiredSet(&stabDesired);
 }
 
 /**
@@ -436,128 +557,128 @@ static void updateFixedAttitude(float* attitude)
  * NED frame as the feedback term and then compares the 
  * @ref VelocityActual against the @ref VelocityDesired
  */
-static void updateVtolDesiredAttitude()
+static void updateVtolDesiredAttitude(bool yaw_attitude)
 {
-	float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
+    float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
 
-	VelocityDesiredData velocityDesired;
+    VelocityDesiredData velocityDesired;
-	VelocityActualData velocityActual;
+    VelocityActualData velocityActual;
-	StabilizationDesiredData stabDesired;
+    StabilizationDesiredData stabDesired;
-	AttitudeActualData attitudeActual;
+    AttitudeActualData attitudeActual;
-	NedAccelData nedAccel;
+    NedAccelData nedAccel;
-	VtolPathFollowerSettingsData vtolpathfollowerSettings;
+    VtolPathFollowerSettingsData vtolpathfollowerSettings;
-	StabilizationSettingsData stabSettings;
+    StabilizationSettingsData stabSettings;
-	SystemSettingsData systemSettings;
+    SystemSettingsData systemSettings;
 
-	float northError;
+    float northError;
-	float northCommand;
+    float northCommand;
-	
-	float eastError;
-	float eastCommand;
 
-	float downError;
+    float eastError;
-	float downCommand;
+    float eastCommand;
-		
+
-	SystemSettingsGet(&systemSettings);
+    float downError;
-	VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
+    float downCommand;
-	
+
-	VelocityActualGet(&velocityActual);
+    SystemSettingsGet(&systemSettings);
-	VelocityDesiredGet(&velocityDesired);
+    VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
-	StabilizationDesiredGet(&stabDesired);
+
-	VelocityDesiredGet(&velocityDesired);
+    VelocityActualGet(&velocityActual);
-	AttitudeActualGet(&attitudeActual);
+    VelocityDesiredGet(&velocityDesired);
-	StabilizationSettingsGet(&stabSettings);
+    StabilizationDesiredGet(&stabDesired);
-	NedAccelGet(&nedAccel);
+    VelocityDesiredGet(&velocityDesired);
-	
+    AttitudeActualGet(&attitudeActual);
-	float northVel = 0, eastVel = 0, downVel = 0;
+    StabilizationSettingsGet(&stabSettings);
-	switch (vtolpathfollowerSettings.VelocitySource) {
+    NedAccelGet(&nedAccel);
-		case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
+
-			northVel = velocityActual.North;
+    float northVel = 0, eastVel = 0, downVel = 0;
-			eastVel = velocityActual.East;
+    switch (vtolpathfollowerSettings.VelocitySource) {
-			downVel = velocityActual.Down;
+        case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
-			break;
+            northVel = velocityActual.North;
-		case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
+            eastVel = velocityActual.East;
-		{
+            downVel = velocityActual.Down;
-			GPSVelocityData gpsVelocity;
+            break;
-			GPSVelocityGet(&gpsVelocity);
+        case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
-			northVel = gpsVelocity.North;
+        {
-			eastVel = gpsVelocity.East;
+            GPSVelocityData gpsVelocity;
-			downVel = gpsVelocity.Down;
+            GPSVelocityGet(&gpsVelocity);
-		}
+            northVel = gpsVelocity.North;
-			break;
+            eastVel = gpsVelocity.East;
-		case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
+            downVel = gpsVelocity.Down;
-		{
+        }
-			GPSPositionData gpsPosition;
+            break;
-			GPSPositionGet(&gpsPosition);
+        case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
+        {
+            GPSPositionData gpsPosition;
+            GPSPositionGet(&gpsPosition);
 			northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading));
 			eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading));
-			downVel = velocityActual.Down;
+            downVel = velocityActual.Down;
-		}
+        }
-			break;
+            break;
-		default:
+        default:
-			PIOS_Assert(0);
+            PIOS_Assert(0);
-			break;
+            break;
-	}
+    }
-	
+
-	// Testing code - refactor into manual control command
+    // Testing code - refactor into manual control command
-	ManualControlCommandData manualControlData;
+    ManualControlCommandData manualControlData;
-	ManualControlCommandGet(&manualControlData);
+    ManualControlCommandGet(&manualControlData);
-	stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;	
+
-	
+    // Compute desired north command
-	// Compute desired north command
+    northError = velocityDesired.North - northVel;
-	northError = velocityDesired.North - northVel;
+    northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-	northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], 
+            -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
-			      -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
+            vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
-			      vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
+    northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
-	northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
+            - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
-			northVelIntegral -
+            + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
-			nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
+
-			velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
+    // Compute desired east command
-	
+    eastError = velocityDesired.East - eastVel;
-	// Compute desired east command
+    eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-	eastError = velocityDesired.East - eastVel;
+            -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
-	eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], 
+            vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
-			     -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
+    eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
-			     vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
+            - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
-	eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + 
+            + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
-		       eastVelIntegral - 
+
-		       nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
+    // Compute desired down command
-			   velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
+    downError = velocityDesired.Down - downVel;
-	
+    // Must flip this sign
-	// Compute desired down command
+    downError = -downError;
-	downError = velocityDesired.Down - downVel;
+    downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-	// Must flip this sign 
+            -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
-	downError = -downError;
+            vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
-	downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], 
+    downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
-			      -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
+            - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
-			      vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);	
+
-	downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] +
+    stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
-		       downVelIntegral -
+
-		       nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
+    // Project the north and east command signals into the pitch and roll based on yaw.  For this to behave well the
-	
+    // craft should move similarly for 5 deg roll versus 5 deg pitch
-	stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
-	
-	// Project the north and east command signals into the pitch and roll based on yaw.  For this to behave well the
-	// craft should move similarly for 5 deg roll versus 5 deg pitch
 	stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) +
 				      -eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)),
-				      -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
+            -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
 	stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) +
 				     eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)),
-				     -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
+            -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
-	
+
-	if(vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
+    if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
-		// For now override throttle with manual control.  Disable at your risk, quad goes to China.
+        // For now override throttle with manual control.  Disable at your risk, quad goes to China.
-		ManualControlCommandData manualControl;
+        ManualControlCommandData manualControl;
-		ManualControlCommandGet(&manualControl);
+        ManualControlCommandGet(&manualControl);
-		stabDesired.Throttle = manualControl.Throttle;
+        stabDesired.Throttle = manualControl.Throttle;
-	}
+    }
-	
+
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+    if (yaw_attitude) {
-	
+        stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-	StabilizationDesiredSet(&stabDesired);
+    } else {
+        stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+        stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
+    }
+    StabilizationDesiredSet(&stabDesired);
 }
 
 /**
@@ -565,39 +686,39 @@ static void updateVtolDesiredAttitude()
  */
 static void updateNedAccel()
 {
-	float accel[3];
+    float accel[3];
-	float q[4];
+    float q[4];
-	float Rbe[3][3];
+    float Rbe[3][3];
-	float accel_ned[3];
+    float accel_ned[3];
 
-	// Collect downsampled attitude data
+    // Collect downsampled attitude data
-	AccelsData accels;
+    AccelsData accels;
-	AccelsGet(&accels);		
+    AccelsGet(&accels);
-	accel[0] = accels.x;
+    accel[0] = accels.x;
-	accel[1] = accels.y;
+    accel[1] = accels.y;
-	accel[2] = accels.z;
+    accel[2] = accels.z;
-	
+
-	//rotate avg accels into earth frame and store it
+    //rotate avg accels into earth frame and store it
-	AttitudeActualData attitudeActual;
+    AttitudeActualData attitudeActual;
-	AttitudeActualGet(&attitudeActual);
+    AttitudeActualGet(&attitudeActual);
-	q[0]=attitudeActual.q1;
+    q[0] = attitudeActual.q1;
-	q[1]=attitudeActual.q2;
+    q[1] = attitudeActual.q2;
-	q[2]=attitudeActual.q3;
+    q[2] = attitudeActual.q3;
-	q[3]=attitudeActual.q4;
+    q[3] = attitudeActual.q4;
-	Quaternion2R(q, Rbe);
+    Quaternion2R(q, Rbe);
-	for (uint8_t i=0; i<3; i++){
+    for (uint8_t i = 0; i < 3; i++) {
-		accel_ned[i]=0;
+        accel_ned[i] = 0;
-		for (uint8_t j=0; j<3; j++)
+        for (uint8_t j = 0; j < 3; j++)
-			accel_ned[i] += Rbe[j][i]*accel[j];
+            accel_ned[i] += Rbe[j][i] * accel[j];
-	}
+    }
-	accel_ned[2] += 9.81f;
+    accel_ned[2] += 9.81f;
-	
+
-	NedAccelData accelData;
+    NedAccelData accelData;
-	NedAccelGet(&accelData);
+    NedAccelGet(&accelData);
-	accelData.North = accel_ned[0];
+    accelData.North = accel_ned[0];
-	accelData.East = accel_ned[1];
+    accelData.East = accel_ned[1];
-	accelData.Down = accel_ned[2];
+    accelData.Down = accel_ned[2];
-	NedAccelSet(&accelData);
+    NedAccelSet(&accelData);
 }
 
 /**
@@ -605,17 +726,41 @@ static void updateNedAccel()
  */
 static float bound(float val, float min, float max)
 {
-	if (val < min) {
+    if (val < min) {
-		val = min;
+        val = min;
-	} else if (val > max) {
+    } else if (val > max) {
-		val = max;
+        val = max;
-	}
+    }
-	return val;
+    return val;
 }
 
-static void SettingsUpdatedCb(UAVObjEvent * ev)
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-	VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
+    VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
-	PathDesiredGet(&pathDesired);
+}
+
+static void accessoryUpdated(UAVObjEvent* ev)
+{
+    if (ev->obj != AccessoryDesiredHandle())
+        return;
+
+    AccessoryDesiredData accessory;
+    PoiLearnSettingsData poiLearn;
+    PoiLearnSettingsGet(&poiLearn);
+
+    if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
+        if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
+            if (accessory.AccessoryVal < -0.5f) {
+                PositionActualData positionActual;
+                PositionActualGet(&positionActual);
+                PoiLocationData poi;
+                PoiLocationGet(&poi);
+                poi.North = positionActual.North;
+                poi.East = positionActual.East;
+                poi.Down = positionActual.Down;
+                PoiLocationSet(&poi);
+            }
+        }
+    }
 }
 

flight/pios/common/libraries/dosfs/dosfs.c

@@ -1194,7 +1194,7 @@ uint32_t DFS_WriteFile(PFILEINFO fileinfo, uint8_t *scratch, uint8_t *buffer, ui
                         // this point, all passes through the read loop will be aligned on a
                         // sector boundary, which allows us to go through the optimal path
                         // 2A below.
-                        if (remain >= SECTOR_SIZE - tempsize) {
+                        if (remain >= (uint32_t)SECTOR_SIZE - tempsize) {
                                 memcpy(scratch + tempsize, buffer, SECTOR_SIZE - tempsize);
                                 if (!result)
                                         result = DFS_WriteSector(fileinfo->volinfo->unit, scratch, sector, 1);
@@ -1330,7 +1330,7 @@ uint32_t DFS_WriteFile(PFILEINFO fileinfo, uint8_t *scratch, uint8_t *buffer, ui
         No original function of DosFS driver
         It has no effect if writing to SD Card, it's only used by the DosFS wrapper in emulation
 */
-uint32_t DFS_Close(PFILEINFO fileinfo)
+uint32_t DFS_Close(__attribute__((unused)) PFILEINFO fileinfo)
 {
   return DFS_OK;
 }

flight/pios/common/libraries/msheap/msheap.c

@@ -423,7 +423,7 @@ msheap_extend(uint32_t size)
  * @param   reason      The reason we are panicking.
  */
 void
-msheap_panic(const char *reason)
+msheap_panic(__attribute__((unused)) const char *reason)
 {
     for (;;)
         ;

flight/pios/common/libraries/msheap/pios_msheap.c

@@ -134,7 +134,7 @@ free(void *p)
 #endif /* PIOS_INCLUDE_FREERTOS */
 
 void
-msheap_panic(const char *reason)
+msheap_panic(__attribute__((unused)) const char *reason)
 {
 	//PIOS_DEBUG_Panic(reason);
 }

flight/pios/common/pios_adxl345.c

@@ -187,7 +187,7 @@ static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth)
 /**
  * @brief Enable measuring.  This also disables the activity sensors (tap or free fall)
  */
-static int32_t PIOS_ADXL345_SetMeasure(uint8_t enable)
+static int32_t PIOS_ADXL345_SetMeasure(__attribute__((unused)) uint8_t enable)
 {
 	if(PIOS_ADXL345_Validate(dev) != 0)
 		return -1;

flight/pios/common/pios_bma180.c

@@ -438,7 +438,7 @@ bool PIOS_BMA180_IRQHandler(void)
 {
 	bma180_irqs++;
 	
-	const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
+	static const uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
 	uint8_t pios_bma180_dmabuf[8];
 
 	// If we can't get the bus then just move on for efficiency

flight/pios/common/pios_bmp085.c

@@ -311,8 +311,8 @@ bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer)
 int32_t PIOS_BMP085_Test()
 {
 	// TODO: Is there a better way to test this than just checking that pressure/temperature has changed?
-	int32_t passed = 1;
+	uint32_t passed = 1;
-	int32_t cur_value = 0;
+	uint32_t cur_value = 0;
 
 	cur_value = Temperature;
 	PIOS_BMP085_StartADC(TemperatureConv);

flight/pios/common/pios_com_msg.c

@@ -76,7 +76,7 @@ int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driv
 	return(0);
 }
 
-static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
+static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield)
 {
 	struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context;
 
@@ -103,7 +103,7 @@ static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint
 	return (bytes_from_fifo);
 }
 
-static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
+static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, __attribute__((unused)) bool * need_yield)
 {
 	struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context;
 

flight/pios/common/pios_flash_jedec.c

@@ -303,12 +303,12 @@ static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id)
 
 #else  /* FLASH_USE_FREERTOS_LOCKS */
 
-static int32_t PIOS_Flash_Jedec_StartTransaction(uintptr_t flash_id)
+static int32_t PIOS_Flash_Jedec_StartTransaction(__attribute__((unused)) uintptr_t flash_id)
 {
 	return 0;
 }
 
-static int32_t PIOS_Flash_Jedec_EndTransaction(uintptr_t flash_id)
+static int32_t PIOS_Flash_Jedec_EndTransaction(__attribute__((unused)) uintptr_t flash_id)
 {
 	return 0;
 }
@@ -387,12 +387,15 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id)
 	while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) {
 #if defined(FLASH_FREERTOS)
 		vTaskDelay(1);
-		if ((i++) % 100 == 0)
+		if ((i++) % 100 == 0) {
 #else
-		if ((i++) % 10000 == 0)
+		if ((i++) % 10000 == 0) {
 #endif
 
+#ifdef PIOS_LED_HEARTBEAT
 			PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif
+		}
 
 }
 

flight/pios/common/pios_flashfs_logfs.c

@@ -192,7 +192,9 @@ static int32_t logfs_erase_all_arenas()
 	uint16_t num_arenas = logfs.cfg->total_fs_size / logfs.cfg->arena_size;
 
 	for (uint16_t arena = 0; arena < num_arenas; arena++) {
-	    PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#ifdef PIOS_LED_HEARTBEAT
+		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif
 		if (logfs_erase_arena(arena) != 0)
 			return -1;
 	}
@@ -815,7 +817,7 @@ static int8_t logfs_append_to_log (uint32_t obj_id, uint16_t obj_inst_id, uint8_
  * @retval -5 if filesystem is full even after garbage collection should have freed space
  * @retval -6 if writing the new object to the filesystem failed
  */
-int32_t PIOS_FLASHFS_ObjSave(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
+int32_t PIOS_FLASHFS_ObjSave(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
 {
 	int8_t rc;
 
@@ -893,7 +895,7 @@ out_exit:
  * @retval -3 if object size in filesystem does not exactly match buffer size
  * @retval -4 if reading the object data from flash fails
  */
-int32_t PIOS_FLASHFS_ObjLoad(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
+int32_t PIOS_FLASHFS_ObjLoad(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t * obj_data, uint16_t obj_size)
 {
 	int8_t rc;
 
@@ -952,7 +954,7 @@ out_exit:
  * @retval -1 if failed to start transaction
  * @retval -2 if failed to delete the object from the filesystem
  */
-int32_t PIOS_FLASHFS_ObjDelete(uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id)
+int32_t PIOS_FLASHFS_ObjDelete(__attribute__((unused)) uint32_t fs_id, uint32_t obj_id, uint16_t obj_inst_id)
 {
 	int8_t rc;
 
@@ -985,7 +987,7 @@ out_exit:
  * @retval -3 if failed to activate arena 0
  * @retval -4 if failed to mount arena 0
  */
-int32_t PIOS_FLASHFS_Format(uint32_t fs_id)
+int32_t PIOS_FLASHFS_Format(__attribute__((unused)) uint32_t fs_id)
 {
 	int32_t rc;
 

flight/pios/common/pios_gcsrcvr.c

@@ -113,7 +113,7 @@ static void gcsreceiver_updated(UAVObjEvent * ev)
 	}
 }
 
-extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
+extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id)
 {
 	struct pios_gcsrcvr_dev *gcsrcvr_dev;
 
@@ -145,7 +145,7 @@ extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
  * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
  * \output >=0 channel value
  */
-static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel)
+static int32_t PIOS_GCSRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel)
 {
 	if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) {
 		/* channel is out of range */

flight/pios/common/pios_hcsr04.c

@@ -29,6 +29,7 @@
  */
 
 #include "pios.h"
+#include "pios_hcsr04_priv.h"
 
 #ifdef PIOS_INCLUDE_HCSR04
 
@@ -37,173 +38,260 @@
 #endif
 
 /* Local Variables */
+/* 100 ms timeout without updates on channels */
+const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
 
-static TIM_ICInitTypeDef TIM_ICInitStructure;
+struct pios_hcsr04_dev * hcsr04_dev_loc;
-static uint8_t CaptureState;
+
-static uint16_t RiseValue;
+enum pios_hcsr04_dev_magic {
-static uint16_t FallValue;
+	PIOS_HCSR04_DEV_MAGIC = 0xab3029AA,
-static uint32_t CaptureValue;
+};
-static uint32_t CapCounter;
+
+struct pios_hcsr04_dev {
+	enum pios_hcsr04_dev_magic     magic;
+	const struct pios_hcsr04_cfg * cfg;
+
+	uint8_t CaptureState[PIOS_PWM_NUM_INPUTS];
+	uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
+	uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
+	uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
+	uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
+	uint32_t us_since_update[PIOS_PWM_NUM_INPUTS];
+};
+
+static bool PIOS_HCSR04_validate(struct pios_hcsr04_dev * hcsr04_dev)
+{
+	return (hcsr04_dev->magic == PIOS_HCSR04_DEV_MAGIC);
+}
+
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_hcsr04_dev * PIOS_PWM_alloc(void)
+{
+	struct pios_hcsr04_dev * hcsr04_dev;
+
+	hcsr04_dev = (struct pios_hcsr04_dev *)pvPortMalloc(sizeof(*hcsr04_dev));
+	if (!hcsr04_dev) return(NULL);
+
+	hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC;
+	return(hcsr04_dev);
+}
+#else
+static struct pios_hcsr04_dev pios_hcsr04_devs[PIOS_PWM_MAX_DEVS];
+static uint8_t pios_hcsr04_num_devs;
+static struct pios_hcsr04_dev * PIOS_PWM_alloc(void)
+{
+	struct pios_hcsr04_dev * hcsr04_dev;
+
+	if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) {
+		return (NULL);
+	}
+
+	hcsr04_dev = &pios_hcsr04_devs[pios_hcsr04_num_devs++];
+	hcsr04_dev->magic = PIOS_HCSR04_DEV_MAGIC;
+
+	return (hcsr04_dev);
+}
+#endif
+
+static void PIOS_HCSR04_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
+static void PIOS_HCSR04_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
+const static struct pios_tim_callbacks tim_callbacks = {
+	.overflow = PIOS_HCSR04_tim_overflow_cb,
+	.edge     = PIOS_HCSR04_tim_edge_cb,
+};
 
-#define PIOS_HCSR04_TRIG_GPIO_PORT                  GPIOD
-#define PIOS_HCSR04_TRIG_PIN                        GPIO_Pin_2
 
 /**
-* Initialise the HC-SR04 sensor
+* Initialises all the pins
 */
-void PIOS_HCSR04_Init(void)
+int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg)
 {
-	/* Init triggerpin */
+	PIOS_DEBUG_Assert(pwm_id);
-	GPIO_InitTypeDef GPIO_InitStructure;
+	PIOS_DEBUG_Assert(cfg);
-	GPIO_StructInit(&GPIO_InitStructure);
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
-	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
-	GPIO_InitStructure.GPIO_Pin = PIOS_HCSR04_TRIG_PIN;
-	GPIO_Init(PIOS_HCSR04_TRIG_GPIO_PORT, &GPIO_InitStructure);
-	PIOS_HCSR04_TRIG_GPIO_PORT->BSRR = PIOS_HCSR04_TRIG_PIN;
 
-	/* Flush counter variables */
+	struct pios_hcsr04_dev * hcsr04_dev;
-	CaptureState = 0;
-	RiseValue = 0;
-	FallValue = 0;
-	CaptureValue = 0;
 
-	/* Setup RCC */
+	hcsr04_dev = (struct pios_hcsr04_dev *) PIOS_PWM_alloc();
-	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+	if (!hcsr04_dev) goto out_fail;
 
-	/* Enable timer interrupts */
+	/* Bind the configuration to the device instance */
-	NVIC_InitTypeDef NVIC_InitStructure;
+	hcsr04_dev->cfg = cfg;
-	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID;
+	hcsr04_dev_loc = hcsr04_dev;
-	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
-	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
-	NVIC_Init(&NVIC_InitStructure);
 
-	/* Partial pin remap for TIM3 (PB5) */
+	for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
-	GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);
+		/* Flush counter variables */
+		hcsr04_dev->CaptureState[i] = 0;
+		hcsr04_dev->RiseValue[i] = 0;
+		hcsr04_dev->FallValue[i] = 0;
+		hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
+	}
 
-	/* Configure input pins */
+	uint32_t tim_id;
-	GPIO_StructInit(&GPIO_InitStructure);
+	if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) {
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
+		return -1;
-	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+	}
-	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
-	GPIO_Init(GPIOB, &GPIO_InitStructure);
 
-	/* Configure timer for input capture */
+	/* Configure the channels to be in capture/compare mode */
-	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+	for (uint8_t i = 0; i < cfg->num_channels; i++) {
-	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+		const struct pios_tim_channel * chan = &cfg->channels[i];
-	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
-	TIM_ICInitStructure.TIM_ICFilter = 0x0;
-	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
-	TIM_ICInit(TIM3, &TIM_ICInitStructure);
 
-	/* Configure timer clocks */
+		/* Configure timer for input capture */
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+		TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
-	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
-	TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
-	TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 500000) - 1;
-	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
-	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-	TIM_InternalClockConfig(TIM3);
-	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
 
-	/* Enable the Capture Compare Interrupt Request */
+		/* Enable the Capture Compare Interrupt Request */
-	//TIM_ITConfig(PIOS_PWM_CH8_TIM_PORT, PIOS_PWM_CH8_CCR, ENABLE);
+		switch (chan->timer_chan) {
-	TIM_ITConfig(TIM3, TIM_IT_CC2, DISABLE);
+		case TIM_Channel_1:
+			TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE);
+			break;
+		case TIM_Channel_2:
+			TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE);
+			break;
+		case TIM_Channel_3:
+			TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE);
+			break;
+		case TIM_Channel_4:
+			TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE);
+			break;
+		}
 
-	/* Enable timers */
+		// Need the update event for that timer to detect timeouts
-	TIM_Cmd(TIM3, ENABLE);
+		TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE);
 
-	/* Setup local variable which stays in this scope */
+	}
-	/* Doing this here and using a local variable saves doing it in the ISR */
+
-	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+#ifndef STM32F4XX
-	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+	/* Enable the peripheral clock for the GPIO */
-	TIM_ICInitStructure.TIM_ICFilter = 0x0;
+	switch ((uint32_t)hcsr04_dev->cfg->trigger.gpio) {
+	case (uint32_t) GPIOA:
+		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
+		break;
+	case (uint32_t) GPIOB:
+		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+		break;
+	case (uint32_t) GPIOC:
+		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
+		break;
+	default:
+		PIOS_Assert(0);
+		break;
+	}
+#endif
+	GPIO_Init(hcsr04_dev->cfg->trigger.gpio, &hcsr04_dev->cfg->trigger.init);
+
+	*pwm_id = (uint32_t) hcsr04_dev;
+
+	return (0);
+
+out_fail:
+	return (-1);
+}
+
+void PIOS_HCSR04_Trigger(void)
+{
+	GPIO_SetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin);
+	PIOS_DELAY_WaituS(15);
+	GPIO_ResetBits(hcsr04_dev_loc->cfg->trigger.gpio,hcsr04_dev_loc->cfg->trigger.init.GPIO_Pin);
 }
 
 /**
-* Get the value of an sonar timer
+* Get the value of an input channel
-* \output >0 timer value
+* \param[in] Channel Number of the channel desired
+* \output -1 Channel not available
+* \output >0 Channel value
 */
 int32_t PIOS_HCSR04_Get(void)
 {
-	return CaptureValue;
+	return hcsr04_dev_loc->CaptureValue[0];
 }
 
-/**
-* Get the value of an sonar timer
-* \output >0 timer value
-*/
 int32_t PIOS_HCSR04_Completed(void)
 {
-	return CapCounter;
+	return hcsr04_dev_loc->CapCounter[0];
-}
-/**
-* Trigger sonar sensor
-*/
-void PIOS_HCSR04_Trigger(void)
-{
-	CapCounter=0;
-	PIOS_HCSR04_TRIG_GPIO_PORT->BSRR = PIOS_HCSR04_TRIG_PIN;
-	PIOS_DELAY_WaituS(15);
-	PIOS_HCSR04_TRIG_GPIO_PORT->BRR = PIOS_HCSR04_TRIG_PIN;
-	TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
 }
 
-
+static void PIOS_HCSR04_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
-/**
-* Handle TIM3 global interrupt request
-*/
-//void PIOS_PWM_irq_handler(TIM_TypeDef * timer)
-void TIM3_IRQHandler(void)
 {
-	/* Zero value always will be changed but this prevents compiler warning */
+	struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context;
-	int32_t i = 0;
 
-	/* Do this as it's more efficient */
+	if (!PIOS_HCSR04_validate(hcsr04_dev)) {
-	if (TIM_GetITStatus(TIM3, TIM_IT_CC2) == SET) {
+		/* Invalid device specified */
-		i = 7;
+		return;
-		if (CaptureState == 0) {
-			RiseValue = TIM_GetCapture2(TIM3);
-		} else {
-			FallValue = TIM_GetCapture2(TIM3);
-		}
 	}
 
-	/* Clear TIM3 Capture compare interrupt pending bit */
+	if (channel >= hcsr04_dev->cfg->num_channels) {
-	TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
+		/* Channel out of range */
+		return;
+	}
 
+	hcsr04_dev->us_since_update[channel] += count;
+	if(hcsr04_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) {
+		hcsr04_dev->CaptureState[channel] = 0;
+		hcsr04_dev->RiseValue[channel] = 0;
+		hcsr04_dev->FallValue[channel] = 0;
+		hcsr04_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT;
+		hcsr04_dev->us_since_update[channel] = 0;
+	}
+
+	return;
+}
+
+static void PIOS_HCSR04_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
+{
+	/* Recover our device context */
+	struct pios_hcsr04_dev * hcsr04_dev = (struct pios_hcsr04_dev *)context;
+
+	if (!PIOS_HCSR04_validate(hcsr04_dev)) {
+		/* Invalid device specified */
+		return;
+	}
+
+	if (chan_idx >= hcsr04_dev->cfg->num_channels) {
+		/* Channel out of range */
+		return;
+	}
+
+	const struct pios_tim_channel * chan = &hcsr04_dev->cfg->channels[chan_idx];
+
+	if (hcsr04_dev->CaptureState[chan_idx] == 0) {
+		hcsr04_dev->RiseValue[chan_idx] = count;
+		hcsr04_dev->us_since_update[chan_idx] = 0;
+	} else {
+		hcsr04_dev->FallValue[chan_idx] = count;
+	}
+
+	// flip state machine and capture value here
 	/* Simple rise or fall state machine */
-	if (CaptureState == 0) {
+	TIM_ICInitTypeDef TIM_ICInitStructure = hcsr04_dev->cfg->tim_ic_init;
+	if (hcsr04_dev->CaptureState[chan_idx] == 0) {
 		/* Switch states */
-		CaptureState = 1;
+		hcsr04_dev->CaptureState[chan_idx] = 1;
 
 		/* Switch polarity of input capture */
 		TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
-		TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
-		TIM_ICInit(TIM3, &TIM_ICInitStructure);
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
-
 	} else {
 		/* Capture computation */
-		if (FallValue > RiseValue) {
+		if (hcsr04_dev->FallValue[chan_idx] > hcsr04_dev->RiseValue[chan_idx]) {
-			CaptureValue = (FallValue - RiseValue);
+			hcsr04_dev->CaptureValue[chan_idx] = (hcsr04_dev->FallValue[chan_idx] - hcsr04_dev->RiseValue[chan_idx]);
 		} else {
-			CaptureValue = ((0xFFFF - RiseValue) + FallValue);
+			hcsr04_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - hcsr04_dev->RiseValue[chan_idx]) + hcsr04_dev->FallValue[chan_idx]);
 		}
 
 		/* Switch states */
-		CaptureState = 0;
+		hcsr04_dev->CaptureState[chan_idx] = 0;
 
 		/* Increase supervisor counter */
-		CapCounter++;
+		hcsr04_dev->CapCounter[chan_idx]++;
-		TIM_ITConfig(TIM3, TIM_IT_CC2, DISABLE);
 
 		/* Switch polarity of input capture */
 		TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
-		TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
-		TIM_ICInit(TIM3, &TIM_ICInitStructure);
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
-
 	}
+
 }
 
 #endif /* PIOS_INCLUDE_HCSR04 */

flight/pios/common/pios_mpu6000.c

@@ -454,7 +454,7 @@ bool PIOS_MPU6000_IRQHandler(void)
 		return false;
 
 	mpu6000_count = PIOS_MPU6000_FifoDepth(true);
-	if (mpu6000_count < sizeof(struct pios_mpu6000_data))
+	if (mpu6000_count < (int32_t)sizeof(struct pios_mpu6000_data))
 		return false;
 
 	if (PIOS_MPU6000_ClaimBus(true) != 0)
@@ -474,7 +474,7 @@ bool PIOS_MPU6000_IRQHandler(void)
 	static struct pios_mpu6000_data data;
 
 	// In the case where extras samples backed up grabbed an extra
-	if (mpu6000_count >= (sizeof(data) * 2)) {
+	if (mpu6000_count >= (int32_t)(sizeof(data) * 2)) {
 		mpu6000_fifo_backup++;
 		if (PIOS_MPU6000_ClaimBus(true) != 0)
 			return false;

flight/pios/common/pios_rfm22b.c

@@ -71,7 +71,6 @@
 #define RFM22B_MAXIMUM_FREQUENCY 440000000
 #define RFM22B_DEFAULT_FREQUENCY 433000000
 #define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000
-//#define RFM22B_TEST_DROPPED_PACKETS 1
 
 // The maximum amount of time since the last message received to consider the connection broken.
 #define DISCONNECT_TIMEOUT_MS 1000 // ms
@@ -125,8 +124,8 @@
 /* Local type definitions */
 
 struct pios_rfm22b_transition {
-    enum pios_rfm22b_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev);
+    enum pios_radio_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev);
-    enum pios_rfm22b_state next_state[RFM22B_EVENT_NUM_EVENTS];
+    enum pios_radio_state next_state[RADIO_EVENT_NUM_EVENTS];
 };
 
 // Must ensure these prefilled arrays match the define sizes
@@ -157,42 +156,39 @@ static const uint8_t OUT_FF[64] = {
 
 /* Local function forwared declarations */
 static void pios_rfm22_task(void *parameters);
-static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
+static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
-static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening);
+static void pios_rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening);
-static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR);
+static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
+static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR);
-static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len);
-static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
-static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event);
+static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event);
-static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event);
+static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev);
+static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev);
-static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status);
-static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len);
 static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size);
 static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel);
 static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev);
-static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev);
 static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev);
+static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev);
 static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks);
-static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev);
 static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev);
 static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22_clearLEDs();
@@ -206,220 +202,187 @@ static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev);
 static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev);
+static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
 static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data);
 static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
-static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr);
 
 
 /* The state transition table */
-const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = {
+static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = {
 
     // Initialization thread
-    [RFM22B_STATE_UNINITIALIZED] = {
+    [RADIO_STATE_UNINITIALIZED] = {
         .entry_fn = 0,
         .next_state = {
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
         },
     },
-    [RFM22B_STATE_INITIALIZING] = {
+    [RADIO_STATE_INITIALIZING] = {
         .entry_fn = rfm22_init,
         .next_state = {
-            [RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_RX_MODE,
+            [RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_REQUESTING_CONNECTION] = {
+    [RADIO_STATE_REQUESTING_CONNECTION] = {
         .entry_fn = rfm22_requestConnection,
         .next_state = {
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR
         },
     },
-    [RFM22B_STATE_ACCEPTING_CONNECTION] = {
+    [RADIO_STATE_ACCEPTING_CONNECTION] = {
         .entry_fn = rfm22_acceptConnection,
         .next_state = {
-            [RFM22B_EVENT_DEFAULT] = RFM22B_STATE_SENDING_ACK,
+            [RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
 
-    [RFM22B_STATE_RX_MODE] = {
+    [RADIO_STATE_RX_MODE] = {
-        .entry_fn = rfm22_setRxMode,
+        .entry_fn = radio_setRxMode,
         .next_state = {
-            [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK,
-            [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK,
+            [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
-            [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
+            [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_WAIT_PREAMBLE] = {
+    [RADIO_STATE_RX_DATA] = {
-        .entry_fn = rfm22_detectPreamble,
+        .entry_fn = radio_rxData,
         .next_state = {
-            [RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC,
+            [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA,
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK,
+            [RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION,
-            [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE,
+            [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK,
-            [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
+            [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK,
+            [RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK,
+            [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_WAIT_SYNC] = {
+    [RADIO_STATE_RECEIVING_ACK] = {
-        .entry_fn = rfm22_detectSync,
-        .next_state = {
-            [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC,
-            [RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA,
-            [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
-        },
-    },
-    [RFM22B_STATE_RX_DATA] = {
-        .entry_fn = rfm22_rxData,
-        .next_state = {
-            [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA,
-            [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
-            [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
-            [RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK,
-            [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS,
-            [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION,
-            [RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK,
-            [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK,
-            [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
-        },
-    },
-    [RFM22B_STATE_RX_FAILURE] = {
-        .entry_fn = rfm22_rxFailure,
-        .next_state = {
-            [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
-        },
-    },
-    [RFM22B_STATE_RECEIVING_ACK] = {
         .entry_fn = rfm22_receiveAck,
         .next_state = {
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
+            [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_RECEIVING_NACK] = {
+    [RADIO_STATE_RECEIVING_NACK] = {
         .entry_fn = rfm22_receiveNack,
         .next_state = {
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_RECEIVING_STATUS] = {
+    [RADIO_STATE_RECEIVING_STATUS] = {
         .entry_fn = rfm22_receiveStatus,
         .next_state = {
-            [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK,
+            [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_TX_START] = {
+    [RADIO_STATE_TX_START] = {
-        .entry_fn = rfm22_txStart,
+        .entry_fn = radio_txStart,
         .next_state = {
-            [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA,
+            [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
-            [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
+            [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_TX_DATA] = {
+    [RADIO_STATE_TX_DATA] = {
-        .entry_fn = rfm22_txData,
+        .entry_fn = radio_txData,
         .next_state = {
-            [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA,
+            [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA,
-            [RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION,
+            [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
-            [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE,
+            [RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE,
-            [RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_TX_FAILURE] = {
+    [RADIO_STATE_TX_FAILURE] = {
         .entry_fn = rfm22_txFailure,
         .next_state = {
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_SENDING_ACK] = {
+    [RADIO_STATE_SENDING_ACK] = {
         .entry_fn = rfm22_sendAck,
         .next_state = {
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_SENDING_NACK] = {
+    [RADIO_STATE_SENDING_NACK] = {
         .entry_fn = rfm22_sendNack,
         .next_state = {
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_TIMEOUT] = {
+    [RADIO_STATE_TIMEOUT] = {
         .entry_fn = rfm22_timeout,
         .next_state = {
-            [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START,
+            [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START,
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_ERROR] = {
+    [RADIO_STATE_ERROR] = {
         .entry_fn = rfm22_error,
         .next_state = {
-            [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR,
+            [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING,
-            [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING,
+            [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR,
-            [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR,
         },
     },
-    [RFM22B_STATE_FATAL_ERROR] = {
+    [RADIO_STATE_FATAL_ERROR] = {
         .entry_fn = rfm22_fatal_error,
         .next_state = {
         },
@@ -511,6 +474,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
     rfm22b_dev->stats.rssi = 0;
     rfm22b_dev->stats.tx_seq = 0;
     rfm22b_dev->stats.rx_seq = 0;
+    rfm22b_dev->stats.tx_failure = 0;
 
     // Initialize the frequencies.
     PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY);
@@ -523,7 +487,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
     rfm22b_dev->coordinator = false;
 
     // Create the event queue
-    rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event));
+    rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event));
 
     // Bind the configuration to the device instance
     rfm22b_dev->cfg = *cfg;
@@ -560,10 +524,10 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu
     initialize_ecc();
 
     // Set the state to initializing.
-    rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED;
+    rfm22b_dev->state = RADIO_STATE_UNINITIALIZED;
 
     // Initialize the radio device.
-    pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false);
+    pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
 
     // Start the driver task.  This task controls the radio state machine and removed all of the IO from the IRQ handler.
     xTaskCreate(pios_rfm22_task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle));
@@ -580,7 +544,7 @@ void PIOS_RFM22B_Reinit(uint32_t rfm22b_id)
 {
     struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
     if (PIOS_RFM22B_Validate(rfm22b_dev)) {
-        pios_rfm22_inject_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false);
+        pios_rfm22_inject_event(rfm22b_dev, RADIO_EVENT_INITIALIZE, false);
     }
 }
 
@@ -594,7 +558,7 @@ bool PIOS_RFM22_EXT_Int(void)
     }
 
     // Inject an interrupt event into the state machine.
-    pios_rfm22_inject_event(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true);
+    pios_rfm22_inject_event(g_rfm22b_dev, RADIO_EVENT_INT_RECEIVED, true);
     return false;
 }
 
@@ -628,6 +592,21 @@ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id)
     return false;
 }
 
+/**
+ * Returns true if the modem is not actively sending or receiving a packet.
+ *
+ * @param[in] rfm22b_id The RFM22B device index.
+ * @return True if the modem is not actively sending or receiving a packet.
+ */
+bool PIOS_RFM22B_InRxWait(uint32_t rfm22b_id)
+{
+    struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
+    if (PIOS_RFM22B_Validate(rfm22b_dev)) {
+        return ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_TRANSITION));
+    }
+    return false;
+}
+
 /**
  * Sets the radio device transmit power.
  *
@@ -790,6 +769,350 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id)
     return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD));
 }
 
+/**
+ * Put the RFM22B device into receive mode.
+ *
+ * @param[in] rfm22b_id  The rfm22b device.
+ * @param[in] p  The packet to receive into.
+ * @return true if Rx mode was entered sucessfully.
+ */
+bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) {
+    struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
+    if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
+        return false;
+    }
+
+    // Are we already in Rx mode?
+    if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT)) {
+        return false;
+    }
+    rfm22b_dev->rx_packet_handle = p;
+
+    // Claim the SPI bus.
+    rfm22_claimBus(rfm22b_dev);
+
+    // disable interrupts
+    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
+    rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
+
+    // Switch to TUNE mode
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
+
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
+    D2_LED_OFF;
+#endif // PIOS_RFM22B_DEBUG_ON_TELEM
+    RX_LED_OFF;
+    TX_LED_OFF;
+
+    // empty the rx buffer
+    rfm22b_dev->rx_buffer_wr = 0;
+
+    // Clear the TX buffer.
+    rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
+
+    // clear FIFOs
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
+
+    // enable RX interrupts
+    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
+                        RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
+    rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval |
+                        RFM22_ie2_enswdet);
+
+    // enable the receiver
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
+
+    // Release the SPI bus.
+    rfm22_releaseBus(rfm22b_dev);
+
+    // Indicate that we're in RX wait mode.
+    rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT;
+
+    return true;
+}
+
+/**
+ * Transmit a packet via the RFM22B device.
+ *
+ * @param[in] rfm22b_id  The rfm22b device.
+ * @param[in] p  The packet to transmit.
+ * @return true if there if the packet was queued for transmission.
+ */
+bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) {
+    struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
+    if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
+        return false;
+    }
+
+    rfm22b_dev->tx_packet = p;
+    rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
+    if (rfm22b_dev->packet_start_ticks == 0) {
+        rfm22b_dev->packet_start_ticks = 1;
+    }
+
+    // Claim the SPI bus.
+    rfm22_claimBus(rfm22b_dev);
+
+    // Disable interrupts
+    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
+    rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
+
+    // set the tx power
+    rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power);
+
+    // TUNE mode
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
+
+    // Queue the data up for sending
+    rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet);
+
+    RX_LED_OFF;
+
+    // Set the destination address in the transmit header.
+    // The destination address is the first 4 bytes of the message.
+    uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
+    rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]);
+    rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]);
+    rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]);
+    rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]);
+
+    // FIFO mode, GFSK modulation
+    uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
+    rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | RFM22_mmc2_modtyp_gfsk);
+
+    // Clear the FIFOs.
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
+
+    // Set the total number of data bytes we are going to transmit.
+    rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr);
+
+    // Add some data to the chips TX FIFO before enabling the transmitter
+    rfm22_assertCs(rfm22b_dev);
+    PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
+    int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
+    bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE:  bytes_to_write;
+    PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
+    rfm22b_dev->tx_data_rd += bytes_to_write;
+    rfm22_deassertCs(rfm22b_dev);
+
+    // Enable TX interrupts.
+    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
+
+    // Enable the transmitter.
+    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
+
+    // Release the SPI bus.
+    rfm22_releaseBus(rfm22b_dev);
+
+    // We're in Tx mode.
+    rfm22b_dev->rfm22b_state = RFM22B_STATE_TX_MODE;
+
+    TX_LED_ON;
+
+    return true;
+}
+
+/**
+ * Process a Tx interrupt from the RFM22B device.
+ *
+ * @param[in] rfm22b_id  The rfm22b device.
+ * @return PIOS_RFM22B_TX_COMPLETE on completed Tx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
+ */
+pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) {
+    struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
+    if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
+        return PIOS_RFM22B_INT_FAILURE;
+    }
+
+    // Read the device status registers
+    if (!pios_rfm22_readStatus(rfm22b_dev)) {
+        return PIOS_RFM22B_INT_FAILURE;
+    }
+
+
+    // TX FIFO almost empty, it needs filling up
+    if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) {
+
+        // Add data to the TX FIFO buffer
+        uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
+        uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
+        rfm22_claimBus(rfm22b_dev);
+        rfm22_assertCs(rfm22b_dev);
+        PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
+        int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
+        bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes:  bytes_to_write;
+        PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
+        rfm22b_dev->tx_data_rd += bytes_to_write;
+        rfm22_deassertCs(rfm22b_dev);
+        rfm22_releaseBus(rfm22b_dev);
+
+        return PIOS_RFM22B_INT_SUCCESS;
+
+    } else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) {
+
+        // Transition out of Tx mode.
+        rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
+
+        return PIOS_RFM22B_TX_COMPLETE;
+    }
+
+    return 0;
+}
+
+/**
+ * Process a Rx interrupt from the RFM22B device.
+ *
+ * @param[in] rfm22b_id  The rfm22b device.
+ * @return PIOS_RFM22B_RX_COMPLETE on completed Rx, or PIOS_RFM22B_INT_SUCCESS/PIOS_RFM22B_INT_FAILURE.
+ */
+pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) {
+    struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
+    if (!PIOS_RFM22B_Validate(rfm22b_dev)) {
+        return PIOS_RFM22B_INT_FAILURE;
+    }
+    uint8_t *rx_buffer = (uint8_t*)(rfm22b_dev->rx_packet_handle);
+    pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS;
+
+    // Read the device status registers
+    if (!pios_rfm22_readStatus(rfm22b_dev)) {
+        rfm22_rxFailure(rfm22b_dev);
+        return PIOS_RFM22B_INT_FAILURE;
+    }
+
+    // FIFO under/over flow error.  Restart RX mode.
+    if (rfm22b_dev->status_regs.int_status_1.fifo_underoverflow_error ||
+        rfm22b_dev->status_regs.int_status_1.crc_error) {
+        rfm22_rxFailure(rfm22b_dev);
+        return PIOS_RFM22B_INT_FAILURE;
+    }
+
+    // Valid packet received
+    if (rfm22b_dev->status_regs.int_status_1.valid_packet_received) {
+
+        // Claim the SPI bus.
+        rfm22_claimBus(rfm22b_dev);
+
+        // read the total length of the packet data
+        uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
+
+        // their must still be data in the RX FIFO we need to get
+        if (rfm22b_dev->rx_buffer_wr < len) {
+            int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
+            // Fetch the data from the RX FIFO
+            rfm22_assertCs(rfm22b_dev);
+            PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
+            rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
+                                                                bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
+            rfm22_deassertCs(rfm22b_dev);
+        }
+	
+        // Release the SPI bus.
+        rfm22_releaseBus(rfm22b_dev);
+
+        // Is there a length error?
+        if (rfm22b_dev->rx_buffer_wr != len) {
+            rfm22_rxFailure(rfm22b_dev);
+            return PIOS_RFM22B_INT_FAILURE;
+        }
+
+        // Increment the total byte received count.
+        rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
+
+        // We're finished with Rx mode
+        rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
+
+        ret = PIOS_RFM22B_RX_COMPLETE;
+
+    } else if (rfm22b_dev->status_regs.int_status_1.rx_fifo_almost_full) {
+
+        // RX FIFO almost full, it needs emptying
+        // read data from the rf chips FIFO buffer
+
+        // Claim the SPI bus.
+        rfm22_claimBus(rfm22b_dev);
+
+        // Read the total length of the packet data
+        uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
+
+        // The received packet is going to be larger than the specified length
+        if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
+            rfm22_releaseBus(rfm22b_dev);
+            rfm22_rxFailure(rfm22b_dev);
+            return PIOS_RFM22B_INT_FAILURE;
+        }
+
+        // Fetch the data from the RX FIFO
+        rfm22_assertCs(rfm22b_dev);
+        PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
+        rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr],
+                                                            RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
+        rfm22_deassertCs(rfm22b_dev);
+
+        // Release the SPI bus.
+        rfm22_releaseBus(rfm22b_dev);
+
+        // Make sure that we're in RX mode.
+        rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
+
+    } else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
+
+        // Valid preamble detected
+        RX_LED_ON;
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
+        D2_LED_ON;
+#endif // PIOS_RFM22B_DEBUG_ON_TELEM
+
+        // We detected the preamble, now wait for sync.
+        rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC;
+
+    } else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) {
+
+        // Sync word detected
+
+        // Claim the SPI bus.
+        rfm22_claimBus(rfm22b_dev);
+
+        // read the 10-bit signed afc correction value
+        // bits 9 to 2
+        uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
+        // bits 1 & 0
+        afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
+        afc_correction >>= 6;
+        // convert the afc value to Hz
+        int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
+        rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
+
+        // read rx signal strength .. 45 = -100dBm, 205 = -20dBm
+        uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
+        // convert to dBm
+        rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
+
+        // Release the SPI bus.
+        rfm22_releaseBus(rfm22b_dev);
+
+        // Indicate that we're in RX mode.
+        rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_MODE;
+
+    } else if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) && !rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) {
+
+        // Waiting for the preamble timed out.
+        rfm22_rxFailure(rfm22b_dev);
+        return PIOS_RFM22B_INT_FAILURE;
+    }
+
+    // Set the packet start time if necessary.
+    if ((rfm22b_dev->packet_start_ticks == 0) &&
+        ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) {
+        rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
+        if (rfm22b_dev->packet_start_ticks == 0)
+            rfm22b_dev->packet_start_ticks = 1;
+    }
+
+    return ret;
+}
+
 /**
  * Validate that the device structure is valid.
  *
@@ -831,10 +1154,10 @@ static void pios_rfm22_task(void *parameters)
             lastEventTicks = xTaskGetTickCount();
 
             // Process events through the state machine.
-            enum pios_rfm22b_event event;
+            enum pios_radio_event event;
             while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
-                if ((event == RFM22B_EVENT_INT_RECEIVED) &&
+                if ((event == RADIO_EVENT_INT_RECEIVED) &&
-                    ((rfm22b_dev->state == RFM22B_STATE_UNINITIALIZED) || (rfm22b_dev->state == RFM22B_STATE_INITIALIZING)))
+                    ((rfm22b_dev->state == RADIO_STATE_UNINITIALIZED) || (rfm22b_dev->state == RADIO_STATE_INITIALIZING)))
                     continue;
                 rfm22_process_event(rfm22b_dev, event);
             }
@@ -843,10 +1166,10 @@ static void pios_rfm22_task(void *parameters)
             portTickType curTicks = xTaskGetTickCount();
             if (pios_rfm22_time_difference_ms(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) {
                 // Transsition through an error event.
-                rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR);
+                rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR);
 
                 // Clear the event queue.
-                enum pios_rfm22b_event event;
+                enum pios_radio_event event;
                 while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) {
                     // Do nothing;
                 }
@@ -855,28 +1178,29 @@ static void pios_rfm22_task(void *parameters)
         }
 
         // Change channels if necessary.
-        if ((rfm22b_dev->state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->state == RFM22B_STATE_WAIT_PREAMBLE)) {
+        if (PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
             rfm22_changeChannel(rfm22b_dev);
         }
 
         portTickType curTicks = xTaskGetTickCount();
         uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks);
-        // Have we been sending this packet too long?
+        // Have we been sending / receiving this packet too long?
-        if ((rfm22b_dev->packet_start_ticks > 0) && (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) {
+        if ((rfm22b_dev->packet_start_ticks > 0) &&
-            rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT);
+            (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) {
-
+            rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT);
-            // Has it been too long since we received a packet
         } else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) {
-            rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR);
+            // Has it been too long since we received a packet
+            rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR);
         } else {
 				
             // Are we waiting for an ACK?
             if (rfm22b_dev->prev_tx_packet) {
 
                 // Should we resend the packet?
-                if (pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) {
+                if ((pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) &&
+                    PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
                     rfm22b_dev->tx_complete_ticks = curTicks;
-                    rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ACK_TIMEOUT);
+                    rfm22_process_event(rfm22b_dev, RADIO_EVENT_ACK_TIMEOUT);
                 }
 
             } else {
@@ -888,7 +1212,7 @@ static void pios_rfm22_task(void *parameters)
                 }
 
                 // Queue up a status packet if it's time.
-                if ((pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) || (last_rec_ms > rfm22b_dev->max_packet_time * 4)) {
+                if (pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) {
                     rfm22_sendStatus(rfm22b_dev);
                     lastStatusTicks = curTicks;
                 }
@@ -897,21 +1221,16 @@ static void pios_rfm22_task(void *parameters)
         }
 
         // Send a packet if it's our time slice
-        rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0x6) == 0);
+        bool time_to_send = rfm22_timeToSend(rfm22b_dev);
-#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
-        if (rfm22b_dev->time_to_send) {
+        if (time_to_send) {
             D4_LED_ON;
         } else {
             D4_LED_OFF;
         }
-        if (rfm22_inChannelBuffer(rfm22b_dev)) {
-            D3_LED_ON;
-        } else {
-            D3_LED_OFF;
-        }
 #endif
-        if (rfm22b_dev->time_to_send) {
+        if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) {
-            rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START);
+            rfm22_process_event(rfm22b_dev, RADIO_EVENT_TX_START);
         }
     }
 }
@@ -928,22 +1247,25 @@ static void pios_rfm22_task(void *parameters)
  * @param[in] event The event to inject
  * @param[in] inISR Is this being called from an interrrup service routine?
  */
-static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR)
+static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR)
 {
-
-    // Store the event.
-    if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
-        return;
-
-    // Signal the semaphore to wake up the handler thread.
     if (inISR) {
-        portBASE_TYPE pxHigherPriorityTaskWoken;
+        // Store the event.
-        if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken) != pdTRUE) {
+        portBASE_TYPE pxHigherPriorityTaskWoken1;
+        if (xQueueSendFromISR(rfm22b_dev->eventQueue, &event, &pxHigherPriorityTaskWoken1) != pdTRUE)
+            return;
+        // Signal the semaphore to wake up the handler thread.
+        portBASE_TYPE pxHigherPriorityTaskWoken2;
+        if (xSemaphoreGiveFromISR(rfm22b_dev->isrPending, &pxHigherPriorityTaskWoken2) != pdTRUE) {
             // Something went fairly seriously wrong
             rfm22b_dev->errors++;
         }
-        portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken);
+        portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
     } else {
+        // Store the event.
+        if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE)
+            return;
+        // Signal the semaphore to wake up the handler thread.
         if (xSemaphoreGive(rfm22b_dev->isrPending) != pdTRUE) {
             // Something went fairly seriously wrong
             rfm22b_dev->errors++;
@@ -956,20 +1278,20 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio
  *
  * @param[in] rfm22b_dev The device structure
  * @param[in] event The event to process
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event)
+static enum pios_radio_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
 {
 
     // No event
-    if (event == RFM22B_EVENT_NUM_EVENTS) {
+    if (event >= RADIO_EVENT_NUM_EVENTS) {
-        return RFM22B_EVENT_NUM_EVENTS;
+        return RADIO_EVENT_NUM_EVENTS;
     }
 
     // Don't transition if there is no transition defined
-    enum pios_rfm22b_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event];
+    enum pios_radio_state next_state = rfm22b_transitions[rfm22b_dev->state].next_state[event];
     if (!next_state) {
-        return RFM22B_EVENT_NUM_EVENTS;
+        return RADIO_EVENT_NUM_EVENTS;
     }
 
     /*
@@ -986,7 +1308,7 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_
         return rfm22b_transitions[rfm22b_dev->state].entry_fn(rfm22b_dev);
     }
 
-    return RFM22B_EVENT_NUM_EVENTS;
+    return RADIO_EVENT_NUM_EVENTS;
 }
 
 /**
@@ -996,10 +1318,10 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_
  * @param[in] rfm22b_dev The device structure
  * @param[in] event The event to process
  */
-static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event)
+static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event)
 {
     // Process all state transitions.
-    while(event != RFM22B_EVENT_NUM_EVENTS) {
+    while(event != RADIO_EVENT_NUM_EVENTS) {
         event = rfm22_process_state_transition(rfm22b_dev, event);
     }
 }
@@ -1013,16 +1335,16 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rf
  * Initialize (or re-initialize) the RFM22B radio device.
  *
  * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
 {
 
     // Initialize the register values.
-    rfm22b_dev->device_status = 0;
+    rfm22b_dev->status_regs.int_status_1.raw = 0;
-    rfm22b_dev->int_status1 = 0;
+    rfm22b_dev->status_regs.int_status_2.raw = 0;
-    rfm22b_dev->int_status2 = 0;
+    rfm22b_dev->status_regs.device_status.raw = 0;
-    rfm22b_dev->ezmac_status = 0;
+    rfm22b_dev->status_regs.ezmac_status.raw = 0;
 
     // Clean the LEDs
     rfm22_clearLEDs();
@@ -1043,8 +1365,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     // Initialize the state
     rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED;
     rfm22b_dev->destination_id = 0xffffffff;
-    rfm22b_dev->time_to_send = false;
-    rfm22b_dev->time_to_send_offset = 0;
     rfm22b_dev->send_status = false;
     rfm22b_dev->send_connection_request = false;
 
@@ -1053,10 +1373,8 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22b_dev->tx_packet = NULL;
     rfm22b_dev->prev_tx_packet = NULL;
     rfm22b_dev->data_packet.header.data_size = 0;
-    rfm22b_dev->in_rx_mode = false;
 
     // Initialize the devide state
-    rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0;
     rfm22b_dev->rx_buffer_wr = 0;
     rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
     rfm22b_dev->frequency_hop_channel = 0;
@@ -1064,27 +1382,31 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22b_dev->packet_start_ticks = 0;
     rfm22b_dev->tx_complete_ticks = 0;
     rfm22b_dev->rx_complete_ticks = 0;
+    rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING;
 
     // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
+    rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
+
+    for (uint8_t i = 0; i < 50; ++i) {
 
-    for (int i = 50; i > 0; i--) {
         // read the status registers
-        rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1);
+        pios_rfm22_readStatus(rfm22b_dev);
-        rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2);
-        if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break;
 
-        // wait 1ms
+        // Is the chip ready?
+        if (rfm22b_dev->status_regs.int_status_2.chip_ready)
+            break;
+
+        // Wait 1ms if not.
         PIOS_DELAY_WaitmS(1);
     }
 
     // ****************
 
     // read status - clears interrupt
-    rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status);
+    pios_rfm22_readStatus(rfm22b_dev);
-    rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1);
+
-    rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2);
+    // Claim the SPI bus.
-    rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status);
+    rfm22_claimBus(rfm22b_dev);
 
     // disable all interrupts
     rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
@@ -1108,14 +1430,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
 #endif
 
         // incorrect RF module type
-        return RFM22B_EVENT_FATAL_ERROR;
+        return RADIO_EVENT_FATAL_ERROR;
     }
     if (device_version != RFM22_DEVICE_VERSION_B1) {
 #if defined(RFM22_DEBUG)
         DEBUG_PRINTF(2, "rf device version: INCORRECT\n\r");
 #endif
         // incorrect RF module version
-        return RFM22B_EVENT_FATAL_ERROR;
+        return RADIO_EVENT_FATAL_ERROR;
     }
 
     // calibrate our RF module to be exactly on frequency .. different for every module
@@ -1182,14 +1504,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
 
     // header control - using a 4 by header with broadcast of 0xffffffff
     rfm22_write(rfm22b_dev, RFM22_header_control1,
-                RFM22_header_cntl1_bcen_0 |
+                        RFM22_header_cntl1_bcen_0 |
-                RFM22_header_cntl1_bcen_1 |
+                        RFM22_header_cntl1_bcen_1 |
-                RFM22_header_cntl1_bcen_2 |
+                        RFM22_header_cntl1_bcen_2 |
-                RFM22_header_cntl1_bcen_3 |
+                        RFM22_header_cntl1_bcen_3 |
-                RFM22_header_cntl1_hdch_0 |
+                        RFM22_header_cntl1_hdch_0 |
-                RFM22_header_cntl1_hdch_1 |
+                        RFM22_header_cntl1_hdch_1 |
-                RFM22_header_cntl1_hdch_2 |
+                        RFM22_header_cntl1_hdch_2 |
-                RFM22_header_cntl1_hdch_3);
+                        RFM22_header_cntl1_hdch_3);
     // Check all bit of all bytes of the header
     rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff);
     rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff);
@@ -1203,9 +1525,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff);
     // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header.
     rfm22_write(rfm22b_dev, RFM22_header_control2,
-                RFM22_header_cntl2_hdlen_3210 |
+                        RFM22_header_cntl2_hdlen_3210 |
-                RFM22_header_cntl2_synclen_3210 |
+                        RFM22_header_cntl2_synclen_3210 |
-                ((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
+                        ((TX_PREAMBLE_NIBBLES >> 8) & 0x01));
 
     // sync word
     rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1);
@@ -1213,9 +1535,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3);
     rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4);
 
-    // set the tx power
-    rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power);
-
     // TX FIFO Almost Full Threshold (0 - 63)
     rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK);
 
@@ -1228,11 +1547,14 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     // Set the frequency calibration
     rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap);
 
+    // Release the bus
+    rfm22_releaseBus(rfm22b_dev);
+
     // Initialize the frequency and datarate to te default.
     rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE);
-    rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true);
+    pios_rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true);
 
-    return RFM22B_EVENT_INITIALIZED;
+    return RADIO_EVENT_INITIALIZED;
 }
 
 /**
@@ -1248,7 +1570,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
  * @param[in] datarate  The air datarate.
  * @param[in] data_whitening  Is data whitening desired?
  */
-static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
+static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
 {
     uint32_t datarate_bps = data_rate[datarate];
     rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f);
@@ -1257,6 +1579,9 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_da
     uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03;
     rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5f) * 4 + 4 + random;
 
+    // Claim the SPI bus.
+    rfm22_claimBus(rfm22b_dev);
+
     // rfm22_if_filter_bandwidth
     rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]);
 
@@ -1309,6 +1634,9 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_da
 
     rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00);
     rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00);
+
+    // Release the bus
+    rfm22_releaseBus(rfm22b_dev);
 }
 
 /**
@@ -1338,6 +1666,9 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
     uint8_t fch = (fc >> 8) & 0xff;
     uint8_t fcl = fc & 0xff;
 
+    // Claim the SPI bus.
+    rfm22_claimBus(rfm22b_dev);
+
     // Calculate the number of frequency hopping channels.
     rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size);
 
@@ -1363,6 +1694,9 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev,
     rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb & 0xff);
     rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fch);
     rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fcl);
+
+    // Release the bus
+    rfm22_releaseBus(rfm22b_dev);
 }
 
 
@@ -1377,22 +1711,20 @@ static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t
     if (rfm22b_dev->frequency_hop_channel == channel) {
         return false;
     }
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
+    D3_LED_TOGGLE;
+#endif // PIOS_RFM22B_DEBUG_ON_TELEM
     rfm22b_dev->frequency_hop_channel = channel;
-    rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel);
+    rfm22_write_claim(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel);
     return true;
 }
 
-
-/*****************************************************************************
- * Radio Transmit and Receive functions.
- *****************************************************************************/
-
 /**
  * Read the RFM22B interrupt and device status registers
  *
  * @param[in] rfm22b_dev  The device structure
  */
-static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
+static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
 {
 
     // 1. Read the interrupt statuses with burst read
@@ -1402,327 +1734,217 @@ static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22_assertCs(rfm22b_dev);
     PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL);
     rfm22_deassertCs(rfm22b_dev);
-    rfm22b_dev->int_status1 = read_buf[1];
+    rfm22b_dev->status_regs.int_status_1.raw = read_buf[1];
-    rfm22b_dev->int_status2 = read_buf[2];
+    rfm22b_dev->status_regs.int_status_2.raw = read_buf[2];
-	
+
     // Device status
-    rfm22b_dev->device_status = rfm22_read_noclaim(rfm22b_dev, RFM22_device_status);
+    rfm22b_dev->status_regs.device_status.raw = rfm22_read(rfm22b_dev, RFM22_device_status);
 
     // EzMAC status
-    rfm22b_dev->ezmac_status = rfm22_read_noclaim(rfm22b_dev, RFM22_ezmac_status);
+    rfm22b_dev->status_regs.ezmac_status.raw = rfm22_read(rfm22b_dev, RFM22_ezmac_status);
 
     // Release the bus
     rfm22_releaseBus(rfm22b_dev);
 
     // the RF module has gone and done a reset - we need to re-initialize the rf module
-    if (rfm22b_dev->int_status2 & RFM22_is2_ipor) {
+    if (rfm22b_dev->status_regs.int_status_2.poweron_reset) {
         return false;
     }
 
     return true;
 }
 
+/**
+ * Recover from a failure in receiving a packet.
+ *
+ * @param[in] rfm22b_dev  The device structure
+ * @return enum pios_radio_event  The next event to inject
+ */
+static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
+{
+    rfm22b_dev->stats.rx_failure++;
+    rfm22b_dev->rx_buffer_wr = 0;
+    rfm22b_dev->packet_start_ticks = 0;
+    rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
+    rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
+    if (rfm22b_dev->rx_complete_ticks == 0) {
+        rfm22b_dev->rx_complete_ticks = 1;
+    }
+}
+
+
+/*****************************************************************************
+ * Radio Transmit and Receive functions.
+ *****************************************************************************/
+
+/**
+ * Start a transmit if possible
+ *
+ * @param[in] radio_dev The device structure
+ * @return enum pios_radio_event  The next event to inject
+ */
+static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
+{
+    PHPacketHandle p = NULL;
+
+    // Don't send if it's not our turn, or if we're receiving a packet.
+    if (!rfm22_timeToSend(radio_dev) || !PIOS_RFM22B_InRxWait((uint32_t)radio_dev)) {
+        return RADIO_EVENT_RX_MODE;
+    }
+
+    // See if there's a packet ready to send.
+    if (radio_dev->tx_packet) {
+        p = radio_dev->tx_packet;
+
+    } else {
+
+        // Don't send a packet if we're waiting for an ACK
+        if (radio_dev->prev_tx_packet) {
+            return RADIO_EVENT_RX_MODE;
+        }
+
+        // Send a connection request?
+        if (!p && radio_dev->send_connection_request) {
+            p = (PHPacketHandle)&(radio_dev->con_packet);
+            radio_dev->send_connection_request = false;
+        }
+
+#ifdef PIOS_PPM_RECEIVER
+        // Send a PPM packet?
+        if (!p && radio_dev->send_ppm)	{
+            p = (PHPacketHandle)&(radio_dev->ppm_packet);
+            radio_dev->send_ppm = false;
+        }
+#endif
+
+        // Send status?
+        if (!p && radio_dev->send_status) {
+            p = (PHPacketHandle)&(radio_dev->status_packet);
+            radio_dev->send_status = false;
+        }
+
+        // Try to get some data to send
+        if (!p)	{
+            bool need_yield = false;
+            p = &radio_dev->data_packet;
+            p->header.type = PACKET_TYPE_DATA;
+            p->header.destination_id = radio_dev->destination_id;
+            if (radio_dev->tx_out_cb && (p->header.data_size == 0)) {
+                p->header.data_size = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield);
+            }
+
+            // Don't send any data until we're connected.
+            if (!rfm22_isConnected(radio_dev)) {
+                p->header.data_size = 0;
+            }
+            if (p->header.data_size == 0) {
+                p = NULL;
+            }
+        }
+    }
+    if (!p) {
+        return RADIO_EVENT_RX_MODE;
+    }
+
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
+    D1_LED_ON;
+#endif
+
+    // Add the packet sequence number.
+    p->header.seq_num = radio_dev->stats.tx_seq++;
+
+    // Pass the time of the previous transmitted packet to use for synchronizing the clocks.
+    p->header.prev_tx_time = radio_dev->tx_complete_ticks;
+
+    // Change the channel if necessary, but not when ACKing the connection request message.
+    if ((p->header.type != PACKET_TYPE_ACK) || (radio_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) {
+        rfm22_changeChannel(radio_dev);
+    }
+
+    // Add the error correcting code.
+    encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
+
+    // Transmit the packet.
+    PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p);
+
+    return RADIO_EVENT_NUM_EVENTS;
+}
+
+/**
+ * Receive packet data.
+ *
+ * @param[in] rfm22b_dev The device structure
+ * @return enum pios_radio_event  The next event to inject
+ */
+static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev)
+{
+    enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
+    pios_rfm22b_int_result res = PIOS_RFM22B_ProcessTx((uint32_t)radio_dev);
+
+    // Is the transmition complete
+    if (res == PIOS_RFM22B_TX_COMPLETE) {
+        radio_dev->stats.tx_byte_count += PH_PACKET_SIZE(radio_dev->tx_packet);
+        radio_dev->tx_complete_ticks = xTaskGetTickCount();
+
+        // Is this an ACK?
+        bool is_ack = (radio_dev->tx_packet->header.type == PACKET_TYPE_ACK);
+        ret_event = RADIO_EVENT_RX_MODE;
+        if (is_ack) {
+
+            // If this is an ACK for a connection request message we need to
+            // configure this modem from the connection request message.
+            if (radio_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
+                rfm22_setConnectionParameters(radio_dev);
+            }
+
+        } else if ((radio_dev->tx_packet->header.type != PACKET_TYPE_NACK) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM) &&
+                   (radio_dev->tx_packet->header.type != PACKET_TYPE_STATUS) && (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM)) {
+
+            // We need to wait for an ACK if this packet it not an ACK, NACK, or PPM.
+            radio_dev->prev_tx_packet = radio_dev->tx_packet;
+
+        }
+        radio_dev->tx_packet = 0;
+        radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0;
+        // Start a new transaction
+        radio_dev->packet_start_ticks = 0;
+
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
+        D1_LED_OFF;
+#endif
+    }
+
+    return ret_event;
+}
+
 /**
  * Switch the radio into receive mode.
  *
  * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev)
 {
-    // Are we already in Rx mode?
+    if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, &(rfm22b_dev->rx_packet))) {
-    if (rfm22b_dev->in_rx_mode) {
+        return RADIO_EVENT_NUM_EVENTS;
-        return RFM22B_EVENT_NUM_EVENTS;
     }
     rfm22b_dev->packet_start_ticks = 0;
-#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
-    D2_LED_ON;
-#endif // PIOS_RFM22B_DEBUG_ON_TELEM
-
-    // disable interrupts
-    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
-    rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
-
-    // Switch to TUNE mode
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
-
-    RX_LED_OFF;
-    TX_LED_OFF;
-
-    // empty the rx buffer
-    rfm22b_dev->rx_buffer_wr = 0;
-
-    // Clear the TX buffer.
-    rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
-
-    // clear FIFOs
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
-
-    // enable RX interrupts
-    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid |
-                RFM22_ie1_enrxffafull | RFM22_ie1_enfferr);
-    rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval |
-                RFM22_ie2_enswdet);
-
-    // enable the receiver
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon);
-
-    // Indicate that we're in RX mode.
-    rfm22b_dev->in_rx_mode = true;
 
     // No event generated
-    return RFM22B_EVENT_NUM_EVENTS;
+    return RADIO_EVENT_NUM_EVENTS;
-}
-
-/**
- * Detect the preamble
- *
- * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
- */
-static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev)
-{
-    // Read the device status registers
-    if (!rfm22_readStatus(rfm22b_dev))
-        return RFM22B_EVENT_FAILURE;
-
-    // Valid preamble detected
-    if (rfm22b_dev->int_status2 & RFM22_is2_ipreaval) {
-        rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
-        if (rfm22b_dev->packet_start_ticks == 0)
-            rfm22b_dev->packet_start_ticks = 1;
-        RX_LED_ON;
-        return RFM22B_EVENT_PREAMBLE_DETECTED;
-    }
-
-    return RFM22B_EVENT_NUM_EVENTS;
-}
-
-/**
- * Detect the sync
- *
- * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
- */
-static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev)
-{
-
-    // Read the device status registers
-    if (!rfm22_readStatus(rfm22b_dev))
-        return RFM22B_EVENT_FAILURE;
-
-    // Sync word detected
-    if (rfm22b_dev->int_status2 & RFM22_is2_iswdet) {
-        RX_LED_ON;
-
-        // read the 10-bit signed afc correction value
-        // bits 9 to 2
-        uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8;
-        // bits 1 & 0
-        afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0;
-        afc_correction >>= 6;
-        // convert the afc value to Hz
-        int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f);
-        rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr);
-
-        // read rx signal strength .. 45 = -100dBm, 205 = -20dBm
-        uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi);
-        // convert to dBm
-        rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122;
-
-        return RFM22B_EVENT_SYNC_DETECTED;
-    } else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) {
-        // Waiting for sync timed out.
-        return RFM22B_EVENT_FAILURE;
-    }
-
-    return RFM22B_EVENT_NUM_EVENTS;
-}
-
-/**
- * Receive the packet data.
- *
- * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
- */
-static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev)
-{
-    // Swap in the next packet buffer if required.
-    uint8_t *rx_buffer = (uint8_t*)&(rfm22b_dev->rx_packet);
-
-    // Read the device status registers
-    if (!rfm22_readStatus(rfm22b_dev)) {
-        return RFM22B_EVENT_FAILURE;
-    }
-
-    // FIFO under/over flow error.  Restart RX mode.
-    if (rfm22b_dev->int_status1 & RFM22_is1_ifferr) {
-        return RFM22B_EVENT_FAILURE;
-    }
-
-    // RX FIFO almost full, it needs emptying
-    if (rfm22b_dev->int_status1 & RFM22_is1_irxffafull) {
-        // read data from the rf chips FIFO buffer
-        // read the total length of the packet data
-        uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
-
-        // The received packet is going to be larger than the specified length
-        if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) {
-            return RFM22B_EVENT_FAILURE;
-        }
-
-        // Another packet length error.
-        if (((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(rfm22b_dev->int_status1 & RFM22_is1_ipkvalid)) {
-            return RFM22B_EVENT_FAILURE;
-        }
-
-        // Fetch the data from the RX FIFO
-        rfm22_claimBus(rfm22b_dev);
-        rfm22_assertCs(rfm22b_dev);
-        PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
-        rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id ,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0;
-        rfm22_deassertCs(rfm22b_dev);
-        rfm22_releaseBus(rfm22b_dev);
-    }
-
-    // CRC error .. discard the received data
-    if (rfm22b_dev->int_status1 & RFM22_is1_icrerror) {
-        return RFM22B_EVENT_FAILURE;
-    }
-
-    // Valid packet received
-    if (rfm22b_dev->int_status1 & RFM22_is1_ipkvalid) {
-
-        // read the total length of the packet data
-        uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length);
-
-        // their must still be data in the RX FIFO we need to get
-        if (rfm22b_dev->rx_buffer_wr < len) {
-            int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr;
-            // Fetch the data from the RX FIFO
-            rfm22_claimBus(rfm22b_dev);
-            rfm22_assertCs(rfm22b_dev);
-            PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F);
-            rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0;
-            rfm22_deassertCs(rfm22b_dev);
-            rfm22_releaseBus(rfm22b_dev);
-        }
-	
-        if (rfm22b_dev->rx_buffer_wr != len) {
-            return RFM22B_EVENT_FAILURE;
-        }
-
-        // we have a valid received packet
-        enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE;
-        if (rfm22b_dev->rx_buffer_wr > 0) {
-            rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr;
-            // Check the packet for errors.
-            if (rfm22_receivePacket(rfm22b_dev, &(rfm22b_dev->rx_packet), rfm22b_dev->rx_buffer_wr)) {
-                switch (rfm22b_dev->rx_packet.header.type) {
-                case PACKET_TYPE_STATUS:
-                    ret_event = RFM22B_EVENT_STATUS_RECEIVED;
-                    break;
-                case PACKET_TYPE_CON_REQUEST:
-                    ret_event = RFM22B_EVENT_CONNECTION_REQUESTED;
-                    break;
-                case PACKET_TYPE_DATA:
-                {
-                    // Send the data to the com port
-                    bool rx_need_yield;
-                    if (rfm22b_dev->rx_in_cb)
-                        (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield);
-#ifdef RFM22B_TEST_DROPPED_PACKETS
-                    // Inject radnom missed ACKs
-                    {
-                        static uint8_t crc = 0;
-                        static uint8_t cntr = 0;
-                        crc = PIOS_CRC_updateByte(crc, cntr++);
-                        if ((crc & 0x7) == 0)
-                            ret_event = RFM22B_EVENT_RX_MODE;
-                    }
-#endif
-                    break;
-                }
-                case PACKET_TYPE_DUPLICATE_DATA:
-                    break;
-                case PACKET_TYPE_ACK:
-                case PACKET_TYPE_ACK_RTS:
-                    ret_event = RFM22B_EVENT_PACKET_ACKED;
-                    break;
-                case PACKET_TYPE_NACK:
-                    ret_event = RFM22B_EVENT_PACKET_NACKED;
-                    break;
-                case PACKET_TYPE_PPM:
-                {
-#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR)
-                    PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet);
-#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT))
-                    bool ppm_output = false;
-#endif
-#endif
-#if defined(PIOS_INCLUDE_RFM22B_RCVR)
-                    ppm_output = true;
-                    for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
-                        rfm22b_dev->ppm_channel[i] = ppmp->channels[i];
-                    }
-#endif					
-#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)
-                    if (PIOS_PPM_OUTPUT) {
-                        ppm_output = true;
-                        for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
-                            PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]);
-                        }
-                    }
-#endif
-#if defined(PIOS_INCLUDE_GCSRCVR)
-                    if (!ppm_output) {
-                        GCSReceiverData gcsRcvr;
-                        for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) {
-                            gcsRcvr.Channel[i] = ppmp->channels[i];
-                        }
-                        GCSReceiverSet(&gcsRcvr);                                        
-                    }
-#endif
-                    break;
-                }
-                default:
-                    break;
-                }
-
-            }
-            else {
-                ret_event = RFM22B_EVENT_RX_ERROR;
-            }
-            rfm22b_dev->rx_buffer_wr = 0;
-            rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
-            if (rfm22b_dev->rx_complete_ticks == 0)
-                rfm22b_dev->rx_complete_ticks = 1;
-#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
-            D2_LED_OFF;
-#endif
-        }
-
-        // We're finished with Rx mode
-        rfm22b_dev->in_rx_mode = false;
-
-        // Start a new transaction
-        rfm22b_dev->packet_start_ticks = 0;
-        return ret_event;
-    }
-
-    return RFM22B_EVENT_NUM_EVENTS;
 }
 
 /**
  * Complete the receipt of a packet.
  *
- * @param[in] rfm22b_dev  The device structure
+ * @param[in] radio_dev  The device structure
  * @param[in] p  The packet handle of the received packet.
  * @param[in] rc_len  The number of bytes received.
+ * @return enum pios_radio_event  The next event to inject
  */
-static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len)
+static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, PHPacketHandle p, uint16_t rx_len)
 {
+    portTickType curTicks = xTaskGetTickCount();
 
     // Attempt to correct any errors in the packet.
     decode_data((unsigned char*)p, rx_len);
@@ -1735,285 +1957,167 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand
         corrected_packet = true;
     }
 
-    // Add any missed packets into the stats.
-    bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_ACK_RTS) || (p->header.type == PACKET_TYPE_NACK));
-    if (!ack_nack_packet && (good_packet || corrected_packet)) {
-        uint16_t seq_num = p->header.seq_num;
-        if (rfm22_isConnected(rfm22b_dev)) {
-            static bool first_time = true;
-            uint16_t missed_packets = 0;
-            if (first_time) {
-                first_time = false;
-            } else {
-                uint16_t prev_seq_num = rfm22b_dev->stats.rx_seq;
-                if (seq_num > prev_seq_num)
-                    missed_packets = seq_num - prev_seq_num - 1;
-                else if((seq_num == prev_seq_num) && (p->header.type == PACKET_TYPE_DATA))
-                    p->header.type = PACKET_TYPE_DUPLICATE_DATA;
-            }
-            rfm22b_dev->stats.rx_missed += missed_packets;
-        }
-        rfm22b_dev->stats.rx_seq = seq_num;
-    }
-
     // Set the packet status
     if (good_packet) {
-        rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET);
+        rfm22b_add_rx_status(radio_dev, RADIO_GOOD_RX_PACKET);
     } else if(corrected_packet) {
         // We corrected the error.
-        rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET);
+        rfm22b_add_rx_status(radio_dev, RADIO_CORRECTED_RX_PACKET);
     } else {
         // We couldn't correct the error, so drop the packet.
-        rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET);
+        rfm22b_add_rx_status(radio_dev, RADIO_ERROR_RX_PACKET);
     }
 
-    return (good_packet || corrected_packet);
+    enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE;
-}
+    if (good_packet || corrected_packet) {
+        switch (p->header.type) {
+        case PACKET_TYPE_STATUS:
+            ret_event = RADIO_EVENT_STATUS_RECEIVED;
 
-/**
+            // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to.
- * Start a transmit if possible
+            if (radio_dev->coordinator && !rfm22_isConnected(radio_dev)) {
- *
+                PHStatusPacketHandle status = (PHStatusPacketHandle)&(radio_dev->rx_packet);
- * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
- */
-static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev)
-{
-    PHPacketHandle p = NULL;
-
-    // Don't send if it's not our turn.
-    if (!rfm22b_dev->time_to_send || (rfm22_inChannelBuffer(rfm22b_dev) && rfm22_isConnected(rfm22b_dev))) {
-        return RFM22B_EVENT_RX_MODE;
-    }
-
-    // See if there's a packet ready to send.
-    if (rfm22b_dev->tx_packet) {
-        p = rfm22b_dev->tx_packet;
-
-    } else {
-
-        // Don't send a packet if we're waiting for an ACK
-        if (rfm22b_dev->prev_tx_packet) {
-            return RFM22B_EVENT_RX_MODE;
-        }
-
-        // Send a connection request?
-        if (!p && rfm22b_dev->send_connection_request) {
-            p = (PHPacketHandle)&(rfm22b_dev->con_packet);
-            rfm22b_dev->send_connection_request = false;
-        }
-
-#ifdef PIOS_PPM_RECEIVER
-        // Send a PPM packet?
-        if (!p && rfm22b_dev->send_ppm)	{
-            p = (PHPacketHandle)&(rfm22b_dev->ppm_packet);
-            rfm22b_dev->send_ppm = false;
-        }
-#endif
-
-        // Send status?
-        if (!p && rfm22b_dev->send_status) {
-            p = (PHPacketHandle)&(rfm22b_dev->status_packet);
-            rfm22b_dev->send_status = false;
-        }
-
-        // Try to get some data to send
-        if (!p)	{
-            bool need_yield = false;
-            p = &rfm22b_dev->data_packet;
-            p->header.type = PACKET_TYPE_DATA;
-            p->header.destination_id = rfm22b_dev->destination_id;
-            if (rfm22b_dev->tx_out_cb && (p->header.data_size == 0)) {
-                p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield);
-            }
-
-            // Don't send any data until we're connected.
-            if (!rfm22_isConnected(rfm22b_dev)) {
-                p->header.data_size = 0;
-            }
-            if (p->header.data_size == 0) {
-                p = NULL;
-            }
-        }
-
-        if (p) {
-            p->header.seq_num = rfm22b_dev->stats.tx_seq++;
-        }
-    }
-    if (!p) {
-        return RFM22B_EVENT_RX_MODE;
-    }
-
-    // We're transitioning out of Rx mode.
-    rfm22b_dev->in_rx_mode = false;
-
-#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
-    D1_LED_ON;
-    D2_LED_OFF;
-#endif
-
-    // Change the channel if necessary.
-    if (((p->header.type != PACKET_TYPE_ACK) && (p->header.type != PACKET_TYPE_ACK_RTS)) ||
-        (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) {
-        rfm22_changeChannel(rfm22b_dev);
-    }
-
-    // Add the error correcting code.
-    encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p);
-
-    rfm22b_dev->tx_packet = p;
-    rfm22b_dev->packet_start_ticks = xTaskGetTickCount();
-    if (rfm22b_dev->packet_start_ticks == 0) {
-        rfm22b_dev->packet_start_ticks = 1;
-    }
-
-    // disable interrupts
-    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00);
-    rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00);
-
-    // TUNE mode
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon);
-
-    // Queue the data up for sending
-    rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet);
-
-    RX_LED_OFF;
-
-    // Set the destination address in the transmit header.
-    // The destination address is the first 4 bytes of the message.
-    uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
-    rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]);
-    rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]);
-    rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]);
-    rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]);
-
-    // FIFO mode, GFSK modulation
-    uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd;
-    rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo |
-                RFM22_mmc2_modtyp_gfsk);
-
-    // clear FIFOs
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx);
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00);
-
-    // *******************
-    // add some data to the chips TX FIFO before enabling the transmitter
-
-    // set the total number of data bytes we are going to transmit
-    rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr);
-
-    // add some data
-    rfm22_claimBus(rfm22b_dev);
-    rfm22_assertCs(rfm22b_dev);
-    PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
-    int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
-    bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE:  bytes_to_write;
-    PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
-    rfm22b_dev->tx_data_rd += bytes_to_write;
-    rfm22_deassertCs(rfm22b_dev);
-    rfm22_releaseBus(rfm22b_dev);
-
-    // enable TX interrupts
-    rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem);
-
-    // enable the transmitter
-    rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon);
-
-    TX_LED_ON;
-
-    return RFM22B_EVENT_NUM_EVENTS;
-}
-
-/**
- * Receive packet data.
- *
- * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
- */
-static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
-{
-    enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS;
-
-    // Read the device status registers
-    if (!rfm22_readStatus(rfm22b_dev)) {
-        return RFM22B_EVENT_FAILURE;
-    }
-
-    // TX FIFO almost empty, it needs filling up
-    if (rfm22b_dev->int_status1 & RFM22_is1_ixtffaem) {
-        // top-up the rf chips TX FIFO buffer
-        uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet);
-        uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1;
-        rfm22_claimBus(rfm22b_dev);
-        rfm22_assertCs(rfm22b_dev);
-        PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80);
-        int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd);
-        bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes:  bytes_to_write;
-        PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL);
-        rfm22b_dev->tx_data_rd += bytes_to_write;
-        rfm22_deassertCs(rfm22b_dev);
-        rfm22_releaseBus(rfm22b_dev);
-
-    // Packet has been sent
-    } else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) {
-        portTickType curTicks = xTaskGetTickCount();
-        rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet);
-
-        // Is this an ACK?
-        bool is_ack = ((rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) || (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS));
-        ret_event = RFM22B_EVENT_RX_MODE;
-        if (is_ack) {
-
-            // If this is an ACK for a connection request message we need to
-            // configure this modem from the connection request message.
-            if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
-
-                rfm22_setConnectionParameters(rfm22b_dev);
-
-            } else if (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) {
-
-                // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to.
-                PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet);
                 uint32_t source_id = status->source_id;
                 for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) {
-                    if (rfm22b_dev->bindings[i].pairID == source_id) {
+                    if (radio_dev->bindings[i].pairID == source_id) {
-                        rfm22b_dev->cur_binding = i;
+                        radio_dev->cur_binding = i;
-                        ret_event = RFM22B_EVENT_REQUEST_CONNECTION;
+                        ret_event = RADIO_EVENT_REQUEST_CONNECTION;
                         break;
                     }
                 }
             }
+            break;
 
-            // Change the channel
+        case PACKET_TYPE_CON_REQUEST:
-            // On the remote side, we initialize the time delta when we finish sending the ACK for the connection request message.
+            ret_event = RADIO_EVENT_CONNECTION_REQUESTED;
-            if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) {
+            break;
-                rfm22b_dev->time_delta = portMAX_DELAY - curTicks;
+        case PACKET_TYPE_DATA:
+        {
+            // Send the data to the com port
+            bool rx_need_yield;
+            if (radio_dev->rx_in_cb)
+                (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p->data, p->header.data_size, NULL, &rx_need_yield);
+            break;
+        }
+        case PACKET_TYPE_DUPLICATE_DATA:
+            break;
+        case PACKET_TYPE_ACK:
+            ret_event = RADIO_EVENT_PACKET_ACKED;
+            break;
+        case PACKET_TYPE_NACK:
+            ret_event = RADIO_EVENT_PACKET_NACKED;
+            break;
+        case PACKET_TYPE_PPM:
+        {
+#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR)
+            PHPpmPacketHandle ppmp = (PHPpmPacketHandle)p;
+#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT))
+            bool ppm_output = false;
+#endif
+#endif
+#if defined(PIOS_INCLUDE_RFM22B_RCVR)
+            ppm_output = true;
+            for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
+                radio_dev->ppm_channel[i] = ppmp->channels[i];
+            }
+#endif					
+#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)
+            if (PIOS_PPM_OUTPUT) {
+                ppm_output = true;
+                for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) {
+                    PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]);
+                }
+            }
+#endif
+#if defined(PIOS_INCLUDE_GCSRCVR)
+            if (!ppm_output) {
+                GCSReceiverData gcsRcvr;
+                for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) {
+                    gcsRcvr.Channel[i] = ppmp->channels[i];
+                }
+                GCSReceiverSet(&gcsRcvr);                                        
+            }
+#endif
+            break;
+        }
+        default:
+            break;
+        }
+
+        uint16_t seq_num = radio_dev->rx_packet.header.seq_num;
+        if (rfm22_isConnected(radio_dev)) {
+
+            // Adjust the clock syncronization if this is the remote modem.
+            // The coordinator sends the time that the previous packet was finised transmitting,
+            // which should match the time that the last packet was received.
+            uint16_t prev_seq_num = radio_dev->stats.rx_seq;
+            if (seq_num == (prev_seq_num + 1)) {
+                portTickType local_rx_time = radio_dev->rx_complete_ticks;
+                portTickType remote_tx_time = radio_dev->rx_packet.header.prev_tx_time;
+                radio_dev->time_delta = remote_tx_time - local_rx_time;
+
+            } else if (seq_num > prev_seq_num) {
+
+                // Add any missed packets into the stats.
+                uint16_t missed_packets = seq_num - prev_seq_num - 1;
+                radio_dev->stats.rx_missed += missed_packets;
             }
 
-        } else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) {
-
-            // We need to wait for an ACK if this packet it not an ACK or NACK.
-            rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet;
-            rfm22b_dev->tx_complete_ticks = xTaskGetTickCount();
         }
-        // Set the Tx period
-        if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) {
-            rfm22b_dev->time_to_send_offset = curTicks + 0x4;
-        } else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) {
-            rfm22b_dev->time_to_send_offset = curTicks;
-        }
-        rfm22b_dev->tx_packet = 0;
-        rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
-        // Start a new transaction
-        rfm22b_dev->packet_start_ticks = 0;
 
-#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
+        // Update the sequence number
-        D1_LED_OFF;
+        radio_dev->stats.rx_seq = seq_num;
-#endif
+
+    } else {
+        ret_event = RADIO_EVENT_RX_COMPLETE;
     }
 
+    // Log the time that the packet was received.
+    radio_dev->rx_complete_ticks = curTicks;
+    if (radio_dev->rx_complete_ticks == 0)
+        radio_dev->rx_complete_ticks = 1;
+
     return ret_event;
 }
 
+/**
+ * Receive the packet data.
+ *
+ * @param[in] rfm22b_dev The device structure
+ * @return enum pios_radio_event  The next event to inject
+ */
+static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev)
+{
+    enum pios_radio_event ret_event = RADIO_EVENT_NUM_EVENTS;
+    pios_rfm22b_int_result res = PIOS_RFM22B_ProcessRx((uint32_t)radio_dev);
+
+    switch (res) {
+
+    case PIOS_RFM22B_RX_COMPLETE:
+
+        // Receive the packet.
+        ret_event = radio_receivePacket(radio_dev, radio_dev->rx_packet_handle, radio_dev->rx_buffer_wr);
+        radio_dev->rx_buffer_wr = 0;
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
+        D2_LED_OFF;
+#endif
+
+        // Start a new transaction
+        radio_dev->packet_start_ticks = 0;
+        break;
+
+    case PIOS_RFM22B_INT_FAILURE:
+
+        ret_event = RADIO_EVENT_RX_MODE;
+        break;
+
+    default:
+        // do nothing.
+        break;
+    }
+
+    return ret_event;
+}
 
 /*****************************************************************************
  * Packet Transmition Functions
@@ -2026,8 +2130,8 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev)
  */
 static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev)
 {
-    // The coordinator doesn't send status.
+    // Don't send if a status is already queued.
-    if (rfm22b_dev->coordinator) {
+    if (rfm22b_dev->send_status) {
         return;
     }
 
@@ -2057,7 +2161,7 @@ static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev)
  *
  * @param[in] rfm22b_dev The device structure
  */
-static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
+static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev)
 {
 #ifdef PIOS_PPM_RECEIVER
     // Only send PPM if we're connected
@@ -2072,9 +2176,9 @@ static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
 
     // See if we have any valid channels.
     bool valid_input_detected = false;
-    for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) {
+    for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; ++i) {
-        rfm22b_dev->ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
+        rfm22b_dev->ppm_packet.channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1);
-        if(rfm22b_dev->ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT)
+        if((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT))
             valid_input_detected = true;
     }
 
@@ -2092,46 +2196,44 @@ static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev)
  * Send an ACK to a received packet.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev)
 {
-    PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
+    // We don't ACK PPM or status packets.
-    aph->header.destination_id = rfm22b_dev->destination_id;
+    if ((rfm22b_dev->rx_packet.header.type != PACKET_TYPE_PPM) && (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_STATUS)) {
-    aph->header.type = rfm22_ready_to_send(rfm22b_dev) ? PACKET_TYPE_ACK_RTS : PACKET_TYPE_ACK;
+        PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
-    aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
+        aph->header.destination_id = rfm22b_dev->destination_id;
-    aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
+        aph->header.type = PACKET_TYPE_ACK;
-    aph->packet_recv_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->rx_complete_ticks);
+        aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
-    rfm22b_dev->tx_packet = (PHPacketHandle)aph;
+        rfm22b_dev->tx_packet = (PHPacketHandle)aph;
-    rfm22b_dev->time_to_send = true;
+    }
-    return RFM22B_EVENT_TX_START;
+    return RADIO_EVENT_TX_START;
 }
 
 /**
  * Send an NACK to a received packet.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev)
 {
     PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet));
     aph->header.destination_id = rfm22b_dev->destination_id;
     aph->header.type = PACKET_TYPE_NACK;
     aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph);
-    aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num;
     rfm22b_dev->tx_packet = (PHPacketHandle)aph;
-    rfm22b_dev->time_to_send = true;
+    return RADIO_EVENT_TX_START;
-    return RFM22B_EVENT_TX_START;
 }
 
 /**
  * Send a connection request message.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev)
 {
     PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet);
 
@@ -2144,15 +2246,15 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf
     cph->header.type = PACKET_TYPE_CON_REQUEST;
     cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet));
     cph->source_id = rfm22b_dev->deviceID;
+    cph->status_rx_time = rfm22b_dev->rx_complete_ticks;
     cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port;
     cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port;
     cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port;
     cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed;
-    rfm22b_dev->time_to_send = true;
     rfm22b_dev->send_connection_request = true;
     rfm22b_dev->prev_tx_packet = NULL;
 
-    return RFM22B_EVENT_TX_START;
+    return RADIO_EVENT_TX_START;
 }
 
 
@@ -2164,12 +2266,11 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf
  * Receive an ACK.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev)
 {
     PHPacketHandle prev = rfm22b_dev->prev_tx_packet;
-    portTickType curTicks = xTaskGetTickCount();
 
     // Clear the previous TX packet.
     rfm22b_dev->prev_tx_packet = NULL;
@@ -2184,36 +2285,17 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de
         break;
     }
 
-    // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message.
-    if (prev->header.type == PACKET_TYPE_CON_REQUEST) {
-        rfm22b_dev->time_delta = portMAX_DELAY - rfm22b_dev->rx_complete_ticks;
-    } else if (!rfm22b_dev->coordinator) {
-        PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->rx_packet));
-        portTickType local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks);
-        portTickType remote_rx_time = aph->packet_recv_time;
-        // Adjust the time delta based on the difference between our estimated time offset and the coordinator offset.
-        // This is not working yet
-        rfm22b_dev->time_delta += remote_rx_time - local_tx_time;
-    }
-
     // Should we try to start another TX?
-    if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) {
+    return RADIO_EVENT_TX_START;
-        rfm22b_dev->time_to_send_offset = curTicks;
-        rfm22b_dev->time_to_send = true;
-        return RFM22B_EVENT_TX_START;
-    } else {
-        rfm22b_dev->time_to_send_offset = curTicks + 0x4;
-        return RFM22B_EVENT_RX_MODE;
-    }
 }
 
 /**
  * Receive an MACK.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev)
 {
 
     // Resend the previous TX packet.
@@ -2222,19 +2304,18 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d
 
     // Increment the reset packet counter if we're connected.
     if (rfm22_isConnected(rfm22b_dev)) {
-        rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET);
+        rfm22b_add_rx_status(rfm22b_dev, RADIO_RESENT_TX_PACKET);
     }
-    rfm22b_dev->time_to_send = true;
+    return RADIO_EVENT_TX_START;
-    return RFM22B_EVENT_TX_START;
 }
 
 /**
  * Receive a status packet
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev)
 {
     PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet);
     int8_t rssi = rfm22b_dev->rssi_dBm;
@@ -2273,7 +2354,7 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b
         rfm22b_dev->pair_stats[min_idx].lastContact = 0;
     }
 
-    return RFM22B_EVENT_RX_COMPLETE;
+    return RADIO_EVENT_RX_COMPLETE;
 }
 
 
@@ -2297,16 +2378,16 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev)
         uint32_t val = rfm22b_dev->rx_packet_stats[i];
         for (uint8_t j = 0; j < 16; ++j) {
             switch ((val >> (j * 2)) & 0x3) {
-            case RFM22B_GOOD_RX_PACKET:
+            case RADIO_GOOD_RX_PACKET:
                 rfm22b_dev->stats.rx_good++;
                 break;
-            case RFM22B_CORRECTED_RX_PACKET:
+            case RADIO_CORRECTED_RX_PACKET:
                 rfm22b_dev->stats.rx_corrected++;
                 break;
-            case RFM22B_ERROR_RX_PACKET:
+            case RADIO_ERROR_RX_PACKET:
                 rfm22b_dev->stats.rx_error++;
                 break;
-            case RFM22B_RESENT_TX_PACKET:
+            case RADIO_RESENT_TX_PACKET:
                 rfm22b_dev->stats.tx_resent++;
                 break;
             }
@@ -2335,39 +2416,6 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r
     rfm22b_dev->rx_packet_stats[0] = (rfm22b_dev->rx_packet_stats[0] << 2) | status;
 }
 
-/**
- * Is it this modem's turn to send?
- *
- * @param[in] rfm22b_dev  The device structure
- */
-static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev)
-{
-    // Is there a status of PPM packet ready to send?
-    if (rfm22b_dev->prev_tx_packet || rfm22b_dev->send_ppm || rfm22b_dev->send_status) {
-        return true;
-    }
-
-    // Are we not connected yet?
-    if (!rfm22_isConnected(rfm22b_dev)) {
-        return true;
-    }
-
-    // Is there some data ready to sent?
-    PHPacketHandle dp = &rfm22b_dev->data_packet;
-    if (dp->header.data_size > 0) {
-        return true;
-    }
-    bool need_yield = false;
-    if (rfm22b_dev->tx_out_cb) {
-        dp->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, PH_MAX_DATA, NULL, &need_yield);
-    }
-    if (dp->header.data_size > 0) {
-        return true;
-    }
-
-    return false;
-}
-
 
 /*****************************************************************************
  * Connection Handling Functions
@@ -2403,16 +2451,16 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev)
 
     // Configure this modem from the connection request message.
     rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing);
-    rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true);
+    pios_rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true);
 }
 
 /**
  * Accept a connection request.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev)
 {
     // Set our connection state to connected
     rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
@@ -2425,7 +2473,13 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm
     // Set the destination ID to the source of the connection request message.
     rfm22b_dev->destination_id = cph->source_id;
 
-    return RFM22B_EVENT_DEFAULT;
+    // The remote modem sets the time delta between the two modems using the differene between the clock
+    // on the local modem when it sent the status packet and the time on the coordinator modem when it was received.
+    portTickType local_tx_time = rfm22b_dev->tx_complete_ticks;
+    portTickType remote_rx_time = cph->status_rx_time;
+    rfm22b_dev->time_delta = remote_rx_time - local_tx_time;
+
+    return RADIO_EVENT_DEFAULT;
 }
 
 
@@ -2433,20 +2487,6 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm
  * Frequency Hopping Functions
  *****************************************************************************/
 
-/**
- * There needs to be a buffer in time around a channel change in which we delay starting a new packet transmit.
- * This function returns true of we are in that range of time.
- *
- * @param[in] rfm22b_dev  The device structure
- * @return True if we're near a channel change time.
- */
-static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev)
-{
-    portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
-    uint8_t window = (uint8_t)(time & 0x7e);
-    return ((window == 0x7f) || (window == 0));
-}
-
 /**
  * Return the extimated current clock ticks count on the coordinator modem.
  * This is the master clock used for all synchronization.
@@ -2455,9 +2495,26 @@ static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev)
  */
 static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks)
 {
+    if (rfm22b_dev->coordinator) {
+        return ticks;
+    }
     return ticks + rfm22b_dev->time_delta;
 }
 
+/**
+ * Return true if this modem is in the send interval, which allows the modem to initiate a transmit.
+ *
+ * @param[in] rfm22b_dev  The device structure
+ */
+static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev)
+{
+    portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
+    // Divide time into 8ms blocks.  Coordinator sends in firs 2 ms, and remote send in 5th and 6th ms.
+    bool tts = (rfm22b_dev->coordinator) ? ((time & 0x06) == 0) : (((time + 4) & 0x06) == 0);
+    // Noone starts a transmit just prior to a channel change.
+    return tts && ((time & 0x7e) < 0x7b);
+}
+
 /**
  * Calculate what the current channel shold be.
  *
@@ -2492,44 +2549,27 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev)
  * Error Handling Functions
  *****************************************************************************/
 
-/**
- * Recover from a failure in receiving a packet.
- *
- * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
- */
-static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev)
-{
-    rfm22b_dev->stats.rx_failure++;
-    rfm22b_dev->rx_buffer_wr = 0;
-    rfm22b_dev->rx_complete_ticks = xTaskGetTickCount();
-    rfm22b_dev->in_rx_mode = false;
-    if (rfm22b_dev->rx_complete_ticks == 0) {
-        rfm22b_dev->rx_complete_ticks = 1;
-    }
-    return RFM22B_EVENT_RX_MODE;
-}
-
 /**
  * Recover from a transmit failure.
  *
  * @param[in] rfm22b_dev The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev)
 {
     rfm22b_dev->stats.tx_failure++;
+    rfm22b_dev->packet_start_ticks = 0;
     rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0;
-    return RFM22B_EVENT_TX_START;
+    return RADIO_EVENT_TX_START;
 }
 
 /**
  * Recover from a timeout event.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
 {
     rfm22b_dev->stats.timeouts++;
     rfm22b_dev->packet_start_ticks = 0;
@@ -2537,29 +2577,30 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev)
     if (rfm22b_dev->tx_packet != 0) {
         rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0;
     }
+    rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION;
     rfm22b_dev->rx_buffer_wr = 0;
     TX_LED_OFF;
     RX_LED_OFF;
-#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
     D1_LED_OFF;
     D2_LED_OFF;
     D3_LED_OFF;
     D4_LED_OFF;
 #endif
-    return RFM22B_EVENT_TX_START;
+    return RADIO_EVENT_RX_MODE;
 }
 
 /**
  * Recover from a severe error.
  *
  * @param[in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
 {
     rfm22b_dev->stats.resets++;
     rfm22_clearLEDs();
-    return RFM22B_EVENT_INITIALIZE;
+    return RADIO_EVENT_INITIALIZE;
 }
 
 /**
@@ -2567,9 +2608,9 @@ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev)
  * this should not happen.
  *
  * @parem [in] rfm22b_dev  The device structure
- * @return enum pios_rfm22b_event  The next event to inject
+ * @return enum pios_radio_event  The next event to inject
  */
-static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev)
+static enum pios_radio_event rfm22_fatal_error(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev)
 {
     // RF module error .. flash the LED's
     rfm22_clearLEDs();
@@ -2593,7 +2634,7 @@ static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_d
 
     PIOS_Assert(0);
 
-    return RFM22B_EVENT_FATAL_ERROR;
+    return RADIO_EVENT_FATAL_ERROR;
 }
 
 
@@ -2659,7 +2700,7 @@ static void rfm22_clearLEDs(void) {
     LINK_LED_OFF;
     RX_LED_OFF;
     TX_LED_OFF;
-#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR)
+#ifdef PIOS_RFM22B_DEBUG_ON_TELEM
     D1_LED_OFF;
     D2_LED_OFF;
     D3_LED_OFF;
@@ -2728,7 +2769,7 @@ static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev)
  * @param[in] addr The address to write to
  * @param[in] data The datat to write to that address
  */
-static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
+static void rfm22_write_claim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
 {
     rfm22_claimBus(rfm22b_dev);
     rfm22_assertCs(rfm22b_dev);
@@ -2739,22 +2780,18 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_
 }
 
 /**
- * Read a byte from an RFM22b register
+ * Write a byte to a register without claiming the semaphore
  *
- * @param[in] rfm22b_dev  The RFM22B device structure pointer.
+ * @param[in] rfm22b_dev  The RFM22B device.
- * @param[in] addr The address to read from
+ * @param[in] addr The address to write to
- * @return Returns the result of the register read
+ * @param[in] data The datat to write to that address
  */
-static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
+static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data)
 {
-    uint8_t in[2];	
-    uint8_t out[2] = {addr & 0x7f, 0xFF};
-    rfm22_claimBus(rfm22b_dev);
     rfm22_assertCs(rfm22b_dev);
-    PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL);
+    uint8_t buf[2] = {addr | 0x80, data};
+    PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL);
     rfm22_deassertCs(rfm22b_dev);
-    rfm22_releaseBus(rfm22b_dev);
-    return in[1];
 }
 
 /**
@@ -2764,7 +2801,7 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
  * @param[in] addr The address to read from
  * @return Returns the result of the register read
  */
-static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
+static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
 {
     uint8_t out[2] = {addr & 0x7F, 0xFF};
     uint8_t in[2];

flight/pios/common/pios_rfm22b_com.c

@@ -91,7 +91,8 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud)
  * @param[in] rfm22b_dev  The device ID.
  * @param[in] rx_bytes_available  The number of bytes available to receive
  */
-static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail)
+static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id,
+									 __attribute__((unused)) uint16_t rx_bytes_avail)
 {
 }
 
@@ -101,7 +102,7 @@ static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail)
  * @param[in] rfm22b_dev  The device ID.
  * @param[in] tx_bytes_available  The number of bytes available to transmit
  */
-static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail)
+static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) uint16_t tx_bytes_avail)
 {
     struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
     if (!PIOS_RFM22B_Validate(rfm22b_dev)) {

flight/pios/common/pios_video.c

@@ -33,8 +33,15 @@
 
 #ifdef PIOS_INCLUDE_VIDEO
 
-extern xSemaphoreHandle osdSemaphore;
+// Private methods
+static void configure_hsync_timers();
+static void stop_hsync_timers();
+static void reset_hsync_timers();
+static void prepare_line(uint32_t line_num);
+static void flush_spi();
 
+// Private variables
+extern xSemaphoreHandle osdSemaphore;
 static const struct pios_video_cfg * dev_cfg;
 
 // Define the buffers.
@@ -46,11 +53,11 @@ static const struct pios_video_cfg * dev_cfg;
 // Must be allocated in one block, so it is in a struct.
 struct _buffers
 {
-        uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
+    uint8_t buffer0_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
-        uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
+    uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
-        uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
+    uint8_t buffer1_level[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
-        uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
+    uint8_t buffer1_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
-} buffers;
+}buffers;
 
 // Remove the struct definition (makes it easier to write for.)
 #define         buffer0_level   (buffers.buffer0_level)
@@ -69,6 +76,7 @@ volatile uint16_t gActiveLine = 0;
 volatile uint16_t gActivePixmapLine = 0;
 volatile uint16_t line=0;
 volatile uint16_t Vsync_update=0;
+volatile uint16_t Hsync_update=0;
 static int16_t m_osdLines=0;
 
 /**
@@ -78,204 +86,395 @@ static int16_t m_osdLines=0;
  */
 void swap_buffers()
 {
-        // While we could use XOR swap this is more reliable and
+    // While we could use XOR swap this is more reliable and
-        // dependable and it's only called a few times per second.
+    // dependable and it's only called a few times per second.
-        // Many compliers should optimise these to EXCH instructions.
+    // Many compliers should optimise these to EXCH instructions.
-        uint8_t *tmp;
+    uint8_t *tmp;
-        SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask);
+    SWAP_BUFFS(tmp, disp_buffer_mask, draw_buffer_mask);
-        SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level);
+    SWAP_BUFFS(tmp, disp_buffer_level, draw_buffer_level);
 }
 
-bool PIOS_Hsync_ISR() {
+bool PIOS_Hsync_ISR()
-
+{
-	if(dev_cfg->hsync->pin.gpio->IDR & dev_cfg->hsync->pin.init.GPIO_Pin) {
+    // On tenth line prepare data which will start clocking out on GRAPHICS_LINE+1
-		//rising
+    if(Hsync_update==GRAPHICS_LINE) {
-		if(gLineType == LINE_TYPE_GRAPHICS)
+        prepare_line(0);
-		{
+        gActiveLine = 1;
-			// Activate new line
+    }
-			DMA_Cmd(dev_cfg->level.dma.tx.channel, ENABLE);
+    Hsync_update++;
-			DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE);
+    return true;
-		}
-	} else {
-		//falling
-		gLineType = LINE_TYPE_UNKNOWN; // Default case
-		gActiveLine++;
-
-		if ((gActiveLine >= GRAPHICS_LINE) && (gActiveLine < (GRAPHICS_LINE + GRAPHICS_HEIGHT))) {
-			gLineType = LINE_TYPE_GRAPHICS;
-			gActivePixmapLine = (gActiveLine - GRAPHICS_LINE);
-			line = gActivePixmapLine*GRAPHICS_WIDTH;
-		}
-
-		if(gLineType == LINE_TYPE_GRAPHICS)
-		{
-			// Load new line
-			DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE);
-			DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE);
-			DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[line],DMA_Memory_0);
-			DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[line],DMA_Memory_0);
-			DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel,BUFFER_LINE_LENGTH);
-			DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel,BUFFER_LINE_LENGTH);
-		}
-	}
-
-	return false;
 }
 
 bool PIOS_Vsync_ISR() {
-	static portBASE_TYPE xHigherPriorityTaskWoken;
+    static portBASE_TYPE xHigherPriorityTaskWoken;
-	//PIOS_LED_Toggle(LED3);
 
-	//if(gActiveLine > 200)
     xHigherPriorityTaskWoken = pdFALSE;
-	m_osdLines = gActiveLine;
+    m_osdLines = gActiveLine;
-	{
-		gActiveLine = 0;
-		Vsync_update++;
-		if(Vsync_update>=2)
-		{
-			swap_buffers();
-			Vsync_update=0;
-			xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken);
-		}
-	}
-	portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); 	//portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
 
-	return xHigherPriorityTaskWoken == pdTRUE;
+    stop_hsync_timers();
+
+    // Wait for previous word to clock out of each
+    TIM_Cmd(dev_cfg->pixel_timer.timer, ENABLE);
+    flush_spi();
+    TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
+
+    gActiveLine = 0;
+    Hsync_update = 0;
+    Vsync_update++;
+    if(Vsync_update>=2)
+    {
+        // load second image buffer
+        swap_buffers();
+        Vsync_update=0;
+
+        // trigger redraw every second field
+        xHigherPriorityTaskWoken = xSemaphoreGiveFromISR(osdSemaphore, &xHigherPriorityTaskWoken);
+    }
+
+    portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); 	//portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
+
+    return xHigherPriorityTaskWoken == pdTRUE;
 }
 
 uint16_t PIOS_Video_GetOSDLines(void) {
-	return m_osdLines;
+    return m_osdLines;
 }
 
-void PIOS_Video_Init(const struct pios_video_cfg * cfg){
+/**
+ * Stops the pixel clock and ensures it ignores the rising edge.  To be used after a 
+ * vsync until the first line is to be displayed
+ */
+static void stop_hsync_timers()
+{
+    // This removes the slave mode configuration
+    TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
+    TIM_InternalClockConfig(dev_cfg->pixel_timer.timer);
+}
 
-	dev_cfg = cfg; // store config before enabling interrupt
+const struct pios_tim_callbacks px_callback = {
+    .overflow = NULL,
+    .edge = NULL,
+};
 
-	if (cfg->mask.remap) {
+#ifdef PAL
-		GPIO_PinAFConfig(cfg->mask.sclk.gpio,
+const uint32_t period = 10;
-				__builtin_ctz(cfg->mask.sclk.init.GPIO_Pin),
+const uint32_t dc = (10 / 2);
-				cfg->mask.remap);
+#else
-		GPIO_PinAFConfig(cfg->mask.mosi.gpio,
+const uint32_t period = 11;
-				__builtin_ctz(cfg->mask.mosi.init.GPIO_Pin),
+const uint32_t dc = (11 / 2);
-				cfg->mask.remap);
+#endif
-	}
+/**
-	if (cfg->level.remap)
+ * Reset the timer and configure for next call.  Keeps them synced.  Ideally this won't even be needed
-	{
+ * since I don't think the slave mode gets lost, and this can simply be disable timer
-		GPIO_PinAFConfig(cfg->level.sclk.gpio,
+ */
-				__builtin_ctz(cfg->level.sclk.init.GPIO_Pin),
+uint32_t failcount = 0;
-				cfg->level.remap);
+static void reset_hsync_timers()
-		GPIO_PinAFConfig(cfg->level.miso.gpio,
+{
-				__builtin_ctz(cfg->level.miso.init.GPIO_Pin),
+    // Stop both timers
-				cfg->level.remap);
+    TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
-	}
 
-	/* SPI3 MASTER MASKBUFFER */
+    uint32_t tim_id;
-	GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init));
+    const struct pios_tim_channel *channels = &dev_cfg->hsync_capture;
-	GPIO_Init(cfg->mask.mosi.gpio, (GPIO_InitTypeDef*)&(cfg->mask.mosi.init));
 
-	/* SPI1 SLAVE FRAMEBUFFER */
+    //BUG: This is nuts this line is needed.  It simply results in allocating
-	GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init));
+    //all the memory but somehow leaving it out breaks the timer functionality.
-	GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init));
+    // I do not see how these can be related
+    if (failcount == 0) {
+        if(PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0) < 0)
+        failcount++;
+    }
 
-	/* Initialize the SPI block */
+    dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; //dc;
-	SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init));
-	SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init));
 
-	/* Enable SPI */
+    // Listen to Channel1 (HSYNC)
-	SPI_Cmd(cfg->level.regs, ENABLE);
+    switch(dev_cfg->hsync_capture.timer_chan) {
-	SPI_Cmd(cfg->mask.regs, ENABLE);
+        case TIM_Channel_1:
+        TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI1FP1);
+        break;
+        case TIM_Channel_2:
+        TIM_SelectInputTrigger(dev_cfg->pixel_timer.timer, TIM_TS_TI2FP2);
+        break;
+        default:
+        PIOS_Assert(0);
+    }
+    TIM_SelectSlaveMode(dev_cfg->pixel_timer.timer, TIM_SlaveMode_Trigger);
+}
 
-	/* Configure DMA for SPI Tx MASTER */
+static void configure_hsync_timers()
-	DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE);
+{
-	DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init));
+    // Stop both timers
+    TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
 
-	/* Configure DMA for SPI Tx SLAVE */
+    // This is overkill but used for consistency.  No interrupts used for pixel clock
-	DMA_Cmd(cfg->level.dma.tx.channel, DISABLE);
+    // but this function calls the GPIO_Remap
-	DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init));
+    uint32_t tim_id;
+    const struct pios_tim_channel *channels;
 
+    // Init the channel to output the pixel clock
+    channels = &dev_cfg->pixel_timer;
+    PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0);
 
-	/* Trigger interrupt when for half conversions too to indicate double buffer */
+    // Init the channel to capture the pulse
-	DMA_ITConfig(cfg->mask.dma.tx.channel, DMA_IT_TC, ENABLE);
+    channels = &dev_cfg->hsync_capture;
-	/*DMA_ClearFlag(cfg->mask.dma.tx.channel,DMA_FLAG_TCIF5);
+    PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0);
-	DMA_ClearITPendingBit(cfg->mask.dma.tx.channel, DMA_IT_TCIF5);
 
-	DMA_ClearFlag(cfg->level.dma.tx.channel,DMA_FLAG_TCIF5);
+    // Configure the input capture channel
-	DMA_ClearITPendingBit(cfg->level.dma.tx.channel, DMA_IT_TCIF5);
+    TIM_ICInitTypeDef TIM_ICInitStructure;
-*/
+    switch(dev_cfg->hsync_capture.timer_chan) {
+        case TIM_Channel_1:
+        TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
+        break;
+        case TIM_Channel_2:
+        TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
+        break;
+        default:
+        PIOS_Assert(0);
+    }
+    TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
+    //TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+    TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+    TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+    TIM_ICInitStructure.TIM_ICFilter = 0;
+    TIM_ICInit(dev_cfg->pixel_timer.timer, &TIM_ICInitStructure);
 
-	/* Configure DMA interrupt */
+    // Set up the channel to output the pixel clock
-	NVIC_Init(&cfg->level.dma.irq.init);
+    switch(dev_cfg->pixel_timer.timer_chan) {
-	NVIC_Init(&cfg->mask.dma.irq.init);
+        case TIM_Channel_1:
+        TIM_OC1Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
+        TIM_OC1PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
+        TIM_SetCompare1(dev_cfg->pixel_timer.timer, dc);
+        break;
+        case TIM_Channel_2:
+        TIM_OC2Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
+        TIM_OC2PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
+        TIM_SetCompare2(dev_cfg->pixel_timer.timer, dc);
+        break;
+        case TIM_Channel_3:
+        TIM_OC3Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
+        TIM_OC3PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
+        TIM_SetCompare3(dev_cfg->pixel_timer.timer, dc);
+        break;
+        case TIM_Channel_4:
+        TIM_OC4Init(dev_cfg->pixel_timer.timer, &dev_cfg->tim_oc_init);
+        TIM_OC4PreloadConfig(dev_cfg->pixel_timer.timer, TIM_OCPreload_Enable);
+        TIM_SetCompare4(dev_cfg->pixel_timer.timer, dc);
+        break;
+    }
+    TIM_ARRPreloadConfig(dev_cfg->pixel_timer.timer, ENABLE);
+    TIM_CtrlPWMOutputs(dev_cfg->pixel_timer.timer, ENABLE);
 
-	/* Enable SPI interrupts to DMA */
+    // This shouldn't be needed as it should come from the config struture.  Something
-	SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
+    // is clobbering that
-	SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
+    TIM_PrescalerConfig(dev_cfg->pixel_timer.timer, 0, TIM_PSCReloadMode_Immediate);
+    TIM_SetAutoreload(dev_cfg->pixel_timer.timer, period);
+}
 
-	/* Configure the Video Line interrupt */
+DMA_TypeDef * main_dma;
-	PIOS_EXTI_Init(cfg->hsync);
+DMA_TypeDef * mask_dma;
-	PIOS_EXTI_Init(cfg->vsync);
+DMA_Stream_TypeDef * main_stream;
+DMA_Stream_TypeDef * mask_stream;
+void PIOS_Video_Init(const struct pios_video_cfg * cfg)
+{
+    dev_cfg = cfg; // store config before enabling interrupt
 
+    configure_hsync_timers();
 
+    /* needed for HW hack */
+    const GPIO_InitTypeDef initStruct = {
+        .GPIO_Pin = GPIO_Pin_12,
+        .GPIO_Speed = GPIO_Speed_100MHz,
+        .GPIO_Mode = GPIO_Mode_IN ,
+        .GPIO_OType = GPIO_OType_PP,
+        .GPIO_PuPd = GPIO_PuPd_NOPULL
+    };
+    GPIO_Init(GPIOC, &initStruct);
+
+    /* SPI3 - MASKBUFFER */
+    GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init));
+    GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef*)&(cfg->mask.miso.init));
+
+    /* SPI1 SLAVE FRAMEBUFFER */
+    GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init));
+    GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init));
+
+    if (cfg->mask.remap) {
+        GPIO_PinAFConfig(cfg->mask.sclk.gpio,
+                __builtin_ctz(cfg->mask.sclk.init.GPIO_Pin),
+                cfg->mask.remap);
+        GPIO_PinAFConfig(cfg->mask.miso.gpio,
+                __builtin_ctz(cfg->mask.miso.init.GPIO_Pin),
+                cfg->mask.remap);
+    }
+    if (cfg->level.remap)
+    {
+        GPIO_PinAFConfig(cfg->level.sclk.gpio,
+                __builtin_ctz(cfg->level.sclk.init.GPIO_Pin),
+                cfg->level.remap);
+        GPIO_PinAFConfig(cfg->level.miso.gpio,
+                __builtin_ctz(cfg->level.miso.init.GPIO_Pin),
+                cfg->level.remap);
+    }
+
+    /* Initialize the SPI block */
+    SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init));
+    SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init));
+
+    /* Enable SPI */
+    SPI_Cmd(cfg->level.regs, ENABLE);
+    SPI_Cmd(cfg->mask.regs, ENABLE);
+
+    /* Configure DMA for SPI Tx SLAVE Maskbuffer */
+    DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE);
+    DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init));
+
+    /* Configure DMA for SPI Tx SLAVE Framebuffer*/
+    DMA_Cmd(cfg->level.dma.tx.channel, DISABLE);
+    DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init));
+
+    /* Trigger interrupt when for half conversions too to indicate double buffer */
+    DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE);
+
+    /* Configure and clear buffers */
     draw_buffer_level = buffer0_level;
     draw_buffer_mask = buffer0_mask;
     disp_buffer_level = buffer1_level;
     disp_buffer_mask = buffer1_mask;
+    memset(disp_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
+    memset(disp_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
+    memset(draw_buffer_mask, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
+    memset(draw_buffer_level, 0, GRAPHICS_WIDTH*GRAPHICS_HEIGHT);
+
+    /* Configure DMA interrupt */
+
+    NVIC_Init(&cfg->level.dma.irq.init);
+
+    /* Enable SPI interrupts to DMA */
+    SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
+    SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
+
+    mask_dma = DMA1;
+    main_dma = DMA2;
+    main_stream = cfg->level.dma.tx.channel;
+    mask_stream = cfg->mask.dma.tx.channel;
+    /* Configure the Video Line interrupt */
+    PIOS_EXTI_Init(cfg->hsync);
+    PIOS_EXTI_Init(cfg->vsync);
+
+    //set levels to zero
+    PIOS_Servo_Set(0,0);
+    PIOS_Servo_Set(1,0);
 }
 
-
 /**
- * @brief Interrupt for half and full buffer transfer
+ * Prepare the system to watch for a HSYNC pulse to trigger the pixel
- *
+ * clock and clock out the next line
- * This interrupt handler swaps between the two halfs of the double buffer to make
- * sure the ahrs uses the most recent data.  Only swaps data when AHRS is idle, but
- * really this is a pretense of a sanity check since the DMA engine is consantly
- * running in the background.  Keep an eye on the ekf_too_slow variable to make sure
- * it's keeping up.
  */
+static void prepare_line(uint32_t line_num)
+{
+    if(line_num<GRAPHICS_HEIGHT)
+    {
+        uint32_t buf_offset = line_num * GRAPHICS_WIDTH;
+
+        dev_cfg->pixel_timer.timer->CNT = dc;
+
+        DMA_ClearFlag(dev_cfg->mask.dma.tx.channel, DMA_FLAG_TCIF7 | DMA_FLAG_HTIF7 | DMA_FLAG_FEIF7 | DMA_FLAG_TEIF7);
+        DMA_ClearFlag(dev_cfg->level.dma.tx.channel, DMA_FLAG_FEIF5 | DMA_FLAG_TEIF5);
+
+        // Load new line
+        DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[buf_offset],DMA_Memory_0);
+        DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[buf_offset],DMA_Memory_0);
+
+        // Enable DMA, Slave first
+        DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel, BUFFER_LINE_LENGTH);
+        DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel, BUFFER_LINE_LENGTH);
+
+        SPI_Cmd(dev_cfg->level.regs, ENABLE);
+        SPI_Cmd(dev_cfg->mask.regs, ENABLE);
+
+        /* Enable SPI interrupts to DMA */
+        SPI_I2S_DMACmd(dev_cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
+        SPI_I2S_DMACmd(dev_cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
+
+        DMA_Cmd(dev_cfg->level.dma.tx.channel, ENABLE);
+        DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE);
+    }
+    reset_hsync_timers();
+}
 
 void PIOS_VIDEO_DMA_Handler(void);
 void DMA1_Stream7_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler")));
 void DMA2_Stream5_IRQHandler(void) __attribute__ ((alias("PIOS_VIDEO_DMA_Handler")));
 
+/**
+ * Check both SPI for the stop sequence before disabling them
+ */
+static void flush_spi()
+{
+    bool level_empty = false;
+    bool mask_empty = false;
+    bool level_stopped = false;
+    bool mask_stopped = false;
 
+    // Can't flush if clock not running
+    while((dev_cfg->pixel_timer.timer->CR1 & 0x0001) && ( !level_stopped | !mask_stopped )) {
+
+        level_empty |= SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == SET;
+        mask_empty |= SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == SET;
+
+        if (level_empty && !level_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == RESET) {
+            SPI_Cmd(dev_cfg->level.regs, DISABLE);
+            level_stopped = true;
+        }
+
+        if (mask_empty && !mask_stopped) { // && SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == RESET) {
+            SPI_Cmd(dev_cfg->mask.regs, DISABLE);
+            mask_stopped = true;
+        }
+    }
+    /*
+     uint32_t i = 0;
+     while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++;
+     while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_TXE) == RESET && i < 30000) i++;
+     while(SPI_I2S_GetFlagStatus(dev_cfg->level.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;
+     while(SPI_I2S_GetFlagStatus(dev_cfg->mask.regs ,SPI_I2S_FLAG_BSY) == SET && i < 30000) i++;*/
+    SPI_Cmd(dev_cfg->mask.regs, DISABLE);
+    SPI_Cmd(dev_cfg->level.regs, DISABLE);
+}
+
+/**
+ * @brief Interrupt for half and full buffer transfer
+ */
 void PIOS_VIDEO_DMA_Handler(void)
 {
-	if (DMA_GetFlagStatus(DMA1_Stream7,DMA_FLAG_TCIF7)) {	// transfer completed load next line
+    // Handle flags from stream channel
-		DMA_ClearFlag(DMA1_Stream7,DMA_FLAG_TCIF7);
+    if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5)) {	// whole double buffer filled
-		//PIOS_LED_Off(LED2);
+        DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5);
-		/*if(gLineType == LINE_TYPE_GRAPHICS)
+        if(gActiveLine < GRAPHICS_HEIGHT)
-		{
+        {
-			// Load new line
+            flush_spi();
-			DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE);
+            stop_hsync_timers();
-			DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE);
-			DMA_MemoryTargetConfig(dev_cfg->level.dma.tx.channel,(uint32_t)&disp_buffer_level[line],DMA_Memory_0);
-			DMA_MemoryTargetConfig(dev_cfg->mask.dma.tx.channel,(uint32_t)&disp_buffer_mask[line],DMA_Memory_0);
-			//DMA_ClearFlag(dev_cfg->mask.dma.tx.channel,DMA_FLAG_TCIF5); // <-- TODO: HARDCODED
-			//DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_TCIF5); // <-- TODO: HARDCODED
-			DMA_SetCurrDataCounter(dev_cfg->level.dma.tx.channel,BUFFER_LINE_LENGTH);
-			DMA_SetCurrDataCounter(dev_cfg->mask.dma.tx.channel,BUFFER_LINE_LENGTH);
-		}*/
-		//PIOS_LED_Toggle(LED2);
-	}
-	else if (DMA_GetFlagStatus(DMA1_Stream7,DMA_FLAG_HTIF7)) {
-		DMA_ClearFlag(DMA1_Stream7,DMA_FLAG_HTIF7);
-	}
-	else {
 
-	}
+            dev_cfg->pixel_timer.timer->CNT = dc;
 
-	if (DMA_GetFlagStatus(DMA2_Stream5,DMA_FLAG_TCIF5)) {	// whole double buffer filled
+            prepare_line(gActiveLine);
-		DMA_ClearFlag(DMA2_Stream5,DMA_FLAG_TCIF5);
+        }
-		//PIOS_LED_Toggle(LED3);
+        else if(gActiveLine >= GRAPHICS_HEIGHT)
-	}
+        {
-	else if (DMA_GetFlagStatus(DMA2_Stream5,DMA_FLAG_HTIF5)) {
+            //last line completed
-		DMA_ClearFlag(DMA2_Stream5,DMA_FLAG_HTIF5);
+            flush_spi();
-	}
+            stop_hsync_timers();
-	else {
-
-	}
 
+            // STOP DMA, master first
+            DMA_Cmd(dev_cfg->mask.dma.tx.channel, DISABLE);
+            DMA_Cmd(dev_cfg->level.dma.tx.channel, DISABLE);
+        }
+        gActiveLine++;
+    }
+    else if (DMA_GetFlagStatus(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5)) {
+        DMA_ClearFlag(dev_cfg->level.dma.tx.channel,DMA_FLAG_HTIF5);
+    }
+    else {
+    }
 }
 
 #endif /* PIOS_INCLUDE_VIDEO */

flight/pios/inc/pios_hcsr04.h

@@ -32,7 +32,6 @@
 #define PIOS_HCSR04_H
 
 /* Public Functions */
-extern void PIOS_HCSR04_Init(void);
 extern int32_t PIOS_HCSR04_Get(void);
 extern int32_t PIOS_HCSR04_Completed(void);
 extern void PIOS_HCSR04_Trigger(void);

flight/pios/inc/pios_hcsr04_priv.h

@@ -0,0 +1,55 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup   PIOS_SERVO Servo Functions
+ * @brief PIOS interface to read and write from servo PWM ports
+ * @{
+ *
+ * @file       pios_servo_priv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Servo private structures.
+ * @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_HCSR04_PRIV_H
+#define PIOS_HCSR04_PRIV_H
+
+#include <pios.h>
+#include <pios_stm32.h>
+
+#include <pios_tim_priv.h>
+
+struct pios_hcsr04_cfg {
+	TIM_ICInitTypeDef tim_ic_init;
+	const struct pios_tim_channel * channels;
+	uint8_t num_channels;
+	struct stm32_gpio trigger;
+};
+
+extern const struct pios_rcvr_driver pios_pwm_rcvr_driver;
+
+extern int32_t PIOS_HCSR04_Init(uint32_t * pwm_id, const struct pios_hcsr04_cfg * cfg);
+
+#endif /* PIOS_HCSR04_PRIV_H */
+
+/**
+ * @}
+ * @}
+ */

flight/pios/inc/pios_rfm22b.h

@@ -75,6 +75,13 @@ enum rfm22b_datarate {
     RFM22_datarate_256000 = 14,
 };
 
+typedef enum {
+    PIOS_RFM22B_INT_FAILURE,
+    PIOS_RFM22B_INT_SUCCESS,
+    PIOS_RFM22B_TX_COMPLETE,
+    PIOS_RFM22B_RX_COMPLETE
+} pios_rfm22b_int_result;
+
 struct rfm22b_stats {
     uint16_t packets_per_sec;
     uint16_t tx_byte_count;
@@ -116,7 +123,12 @@ extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id);
 extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id);
 extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats);
 extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs);
+extern bool PIOS_RFM22B_InRxWait(uint32_t rfb22b_id);
 extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id);
+extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p);
+extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p);
+extern pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id);
+extern pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id);
 
 /* Global Variables */
 extern const struct pios_com_driver pios_rfm22b_com_driver;

flight/pios/inc/pios_rfm22b_priv.h

@@ -531,62 +531,69 @@ enum pios_rfm22b_dev_magic {
     PIOS_RFM22B_DEV_MAGIC = 0x68e971b6,
 };
 
+enum pios_radio_state {
+    RADIO_STATE_UNINITIALIZED,
+    RADIO_STATE_INITIALIZING,
+    RADIO_STATE_REQUESTING_CONNECTION,
+    RADIO_STATE_ACCEPTING_CONNECTION,
+    RADIO_STATE_RX_MODE,
+    RADIO_STATE_RX_DATA,
+    RADIO_STATE_RX_FAILURE,
+    RADIO_STATE_RECEIVING_STATUS,
+    RADIO_STATE_TX_START,
+    RADIO_STATE_TX_DATA,
+    RADIO_STATE_TX_FAILURE,
+    RADIO_STATE_SENDING_ACK,
+    RADIO_STATE_SENDING_NACK,
+    RADIO_STATE_RECEIVING_ACK,
+    RADIO_STATE_RECEIVING_NACK,
+    RADIO_STATE_TIMEOUT,
+    RADIO_STATE_ERROR,
+    RADIO_STATE_FATAL_ERROR,
+
+    RADIO_STATE_NUM_STATES // Must be last
+};
+
+enum pios_radio_event {
+    RADIO_EVENT_DEFAULT,
+    RADIO_EVENT_INT_RECEIVED,
+    RADIO_EVENT_INITIALIZE,
+    RADIO_EVENT_INITIALIZED,
+    RADIO_EVENT_REQUEST_CONNECTION,
+    RADIO_EVENT_CONNECTION_REQUESTED,
+    RADIO_EVENT_PACKET_ACKED,
+    RADIO_EVENT_PACKET_NACKED,
+    RADIO_EVENT_ACK_TIMEOUT,
+    RADIO_EVENT_RX_MODE,
+    RADIO_EVENT_RX_COMPLETE,
+    RADIO_EVENT_STATUS_RECEIVED,
+    RADIO_EVENT_TX_START,
+    RADIO_EVENT_FAILURE,
+    RADIO_EVENT_TIMEOUT,
+    RADIO_EVENT_ERROR,
+    RADIO_EVENT_FATAL_ERROR,
+
+    RADIO_EVENT_NUM_EVENTS  // Must be last
+};
+
 enum pios_rfm22b_state {
-    RFM22B_STATE_UNINITIALIZED,
     RFM22B_STATE_INITIALIZING,
-    RFM22B_STATE_REQUESTING_CONNECTION,
+    RFM22B_STATE_TRANSITION,
-    RFM22B_STATE_ACCEPTING_CONNECTION,
+    RFM22B_STATE_RX_WAIT,
+    RFM22B_STATE_RX_WAIT_SYNC,
     RFM22B_STATE_RX_MODE,
-    RFM22B_STATE_WAIT_PREAMBLE,
+    RFM22B_STATE_TX_MODE,
-    RFM22B_STATE_WAIT_SYNC,
+    RFM22B_STATE_TRANSMITTING,
-    RFM22B_STATE_RX_DATA,
-    RFM22B_STATE_RX_FAILURE,
-    RFM22B_STATE_RECEIVING_STATUS,
-    RFM22B_STATE_TX_START,
-    RFM22B_STATE_TX_DATA,
-    RFM22B_STATE_TX_FAILURE,
-    RFM22B_STATE_SENDING_ACK,
-    RFM22B_STATE_SENDING_NACK,
-    RFM22B_STATE_RECEIVING_ACK,
-    RFM22B_STATE_RECEIVING_NACK,
-    RFM22B_STATE_TIMEOUT,
-    RFM22B_STATE_ERROR,
-    RFM22B_STATE_FATAL_ERROR,
 
     RFM22B_STATE_NUM_STATES // Must be last
 };
 
-enum pios_rfm22b_event {
-    RFM22B_EVENT_DEFAULT,
-    RFM22B_EVENT_INT_RECEIVED,
-    RFM22B_EVENT_INITIALIZE,
-    RFM22B_EVENT_INITIALIZED,
-    RFM22B_EVENT_REQUEST_CONNECTION,
-    RFM22B_EVENT_CONNECTION_REQUESTED,
-    RFM22B_EVENT_PACKET_ACKED,
-    RFM22B_EVENT_PACKET_NACKED,
-    RFM22B_EVENT_ACK_TIMEOUT,
-    RFM22B_EVENT_RX_MODE,
-    RFM22B_EVENT_PREAMBLE_DETECTED,
-    RFM22B_EVENT_SYNC_DETECTED,
-    RFM22B_EVENT_RX_COMPLETE,
-    RFM22B_EVENT_RX_ERROR,
-    RFM22B_EVENT_STATUS_RECEIVED,
-    RFM22B_EVENT_TX_START,
-    RFM22B_EVENT_FAILURE,
-    RFM22B_EVENT_TIMEOUT,
-    RFM22B_EVENT_ERROR,
-    RFM22B_EVENT_FATAL_ERROR,
-
-    RFM22B_EVENT_NUM_EVENTS  // Must be last
-};
-
 #define RFM22B_RX_PACKET_STATS_LEN 4
 enum pios_rfm22b_rx_packet_status {
-    RFM22B_GOOD_RX_PACKET = 0x00,
+    RADIO_GOOD_RX_PACKET = 0x00,
-    RFM22B_CORRECTED_RX_PACKET = 0x01,
+    RADIO_CORRECTED_RX_PACKET = 0x01,
-    RFM22B_ERROR_RX_PACKET = 0x2,
+    RADIO_ERROR_RX_PACKET = 0x2,
-    RFM22B_RESENT_TX_PACKET = 0x3
+    RADIO_RESENT_TX_PACKET = 0x3
 };
 
 typedef struct {
@@ -604,6 +611,79 @@ typedef struct {
     OPLinkSettingsComSpeedOptions com_speed;
 } rfm22b_binding;
 
+enum pios_rfm22b_chip_power_state {
+    RFM22B_IDLE_STATE = 0x00,
+    RFM22B_RX_STATE = 0x01,
+    RFM22B_TX_STATE = 0x10,
+    RFM22B_INVALID_STATE = 0x11
+};
+
+// Device Status
+typedef union {
+    struct {
+        uint8_t state : 2;
+        bool frequency_error : 1;
+        bool header_error : 1;
+        bool rx_fifo_empty : 1;
+        bool fifo_underflow : 1;
+        bool fifo_overflow : 1;
+    };
+    uint8_t raw;
+} rfm22b_device_status_reg;
+
+// EzMAC Status
+typedef union {
+    struct {
+        bool packet_sent : 1;
+        bool packet_transmitting : 1;
+        bool crc_error : 1;
+        bool valid_packet_received : 1;
+        bool packet_receiving : 1;
+        bool packet_searching : 1;
+        bool crc_is_all_ones : 1;
+        bool reserved;
+    };
+    uint8_t raw;
+} rfm22b_ezmac_status_reg;
+
+// Interrrupt Status Register 1
+typedef union {
+    struct {
+        bool crc_error : 1;
+        bool valid_packet_received : 1;
+        bool packet_sent_interrupt : 1;
+        bool external_interrupt : 1;
+        bool rx_fifo_almost_full : 1;
+        bool tx_fifo_almost_empty : 1;
+        bool tx_fifo_almost_full : 1;
+        bool fifo_underoverflow_error : 1;
+    };
+    uint8_t raw;
+} rfm22b_int_status_1;
+
+// Interrupt Status Register 2
+typedef union {
+    struct {
+        bool poweron_reset : 1;
+        bool chip_ready : 1;
+        bool low_battery_detect : 1;
+        bool wakeup_timer : 1;
+        bool rssi_above_threshold : 1;
+        bool invalid_preamble_detected : 1;
+        bool valid_preamble_detected : 1;
+        bool sync_word_detected : 1;
+    };
+    uint8_t raw;
+} rfm22b_int_status_2;
+
+typedef struct {
+    rfm22b_device_status_reg device_status;
+    rfm22b_device_status_reg ezmac_status;
+    rfm22b_int_status_1 int_status_1;
+    rfm22b_int_status_2 int_status_2;
+} rfm22b_device_status;
+
+
 struct pios_rfm22b_dev {
     enum pios_rfm22b_dev_magic magic;
     struct pios_rfm22b_cfg cfg;
@@ -644,30 +724,27 @@ struct pios_rfm22b_dev {
     uint32_t tx_out_context;
 
     // the transmit power to use for data transmissions
-    uint8_t	tx_power;
+    uint8_t tx_power;
 
     // The RF datarate lookup index.
     uint8_t datarate;
 
-    // The state machine state and the current event
+    // The radio state machine state
-    enum pios_rfm22b_state state;
+    enum pios_radio_state state;
 
     // The event queue handle
     xQueueHandle eventQueue;
 
-    // device status register
+    // The device status registers.
-    uint8_t device_status;
+    rfm22b_device_status status_regs;
-    // interrupt status register 1
-    uint8_t int_status1;
-    // interrupt status register 2
-    uint8_t int_status2;
-    // ezmac status register
-    uint8_t ezmac_status;
 
     // The error statistics counters
     uint16_t prev_rx_seq_num;
     uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN];
 
+    // The RFM22B state machine state
+    enum pios_rfm22b_state rfm22b_state;
+
     // The packet statistics
     struct rfm22b_stats stats;
 
@@ -692,12 +769,12 @@ struct pios_rfm22b_dev {
 
     // The rx data packet
     PHPacket rx_packet;
+    // The rx data packet
+    PHPacketHandle rx_packet_handle;
     // The receive buffer write index
     uint16_t rx_buffer_wr;
     // The receive buffer write index
     uint16_t rx_packet_len;
-    // Is the modem currently in Rx mode?
-    bool in_rx_mode;
 
     // The status packet
     PHStatusPacket status_packet;
@@ -717,10 +794,6 @@ struct pios_rfm22b_dev {
     bool send_status;
     bool send_ppm;
     bool send_connection_request;
-    bool time_to_send;
-
-    // The offset between our clock and the global send clock
-    uint8_t time_to_send_offset;
 
     // The initial frequency
     uint32_t init_frequency;
@@ -730,7 +803,7 @@ struct pios_rfm22b_dev {
     // The frequency hopping step size
     float frequency_step_size;
     // current frequency hop channel
-    uint8_t	frequency_hop_channel;
+    uint8_t frequency_hop_channel;
     // afc correction reading (in Hz)
     int8_t afc_correction_Hz;
 
@@ -749,7 +822,7 @@ struct pios_rfm22b_dev {
 #ifdef PIOS_INCLUDE_RFM22B_RCVR
     // The PPM channel values
     uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS];
-    uint8_t ppm_supv_timer;
+    uint32_t ppm_supv_timer;
     bool ppm_fresh;
 #endif
 };

flight/pios/inc/pios_video.h

@@ -42,12 +42,10 @@ struct pios_video_cfg {
 	const struct pios_exti_cfg * hsync;
 	const struct pios_exti_cfg * vsync;
 
-	/*struct stm32_exti hsync;
+	struct pios_tim_channel pixel_timer;
-	struct stm32_exti vsync;
+	struct pios_tim_channel hsync_capture;
-	struct stm32_gpio hsync_io;
+
-	struct stm32_gpio vsync_io;
+	TIM_OCInitTypeDef tim_oc_init;
-	struct stm32_irq hsync_irq;
-	struct stm32_irq vsync_irq;*/
 };
 
 // Time vars
@@ -65,20 +63,21 @@ extern bool PIOS_Hsync_ISR();
 extern bool PIOS_Vsync_ISR();
 
 // First OSD line
-#define GRAPHICS_LINE 32
+#define GRAPHICS_LINE 25
 
 //top/left deadband
-#define GRAPHICS_HDEADBAND 32
+#define GRAPHICS_HDEADBAND 80
 #define GRAPHICS_VDEADBAND 0
 
 #define PAL
 
 // Real OSD size
 #ifdef PAL
- #define GRAPHICS_WIDTH_REAL (336+GRAPHICS_HDEADBAND)
+//#define GRAPHICS_WIDTH_REAL (352+GRAPHICS_HDEADBAND)
+#define GRAPHICS_WIDTH_REAL 416
  #define GRAPHICS_HEIGHT_REAL (270+GRAPHICS_VDEADBAND)
 #else
- #define GRAPHICS_WIDTH_REAL (320+GRAPHICS_HDEADBAND)
+ #define GRAPHICS_WIDTH_REAL (312+GRAPHICS_HDEADBAND)
  #define GRAPHICS_HEIGHT_REAL (225+GRAPHICS_VDEADBAND)
 #endif
 

flight/pios/pios.h

@@ -51,11 +51,11 @@
 #include <math.h>
 
 /* STM32 Std Peripherals Lib */
-#ifdef STM32F4XX
+#if defined(STM32F10X)
+#include <stm32f10x.h>
+#elif defined(STM32F4XX)
 #include <stm32f4xx.h>
 #include <stm32f4xx_rcc.h>
-#else
-#include <stm32f10x.h>
 #endif
 
 /* PIOS board specific feature selection */
@@ -67,6 +67,7 @@
 /* PIOS debug interface */
 /* #define PIOS_INCLUDE_DEBUG_CONSOLE */
 /* #define DEBUG_LEVEL 0 */
+/* #define PIOS_ENABLE_DEBUG_PINS */
 #include <pios_debug.h>
 
 /* PIOS common functions */

flight/pios/stm32f10x/pios_debug.c

@@ -7,7 +7,7 @@
  * @{
  *
  * @file       pios_debug.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
  * @brief      Debugging Functions
  * @see        The GNU Public License (GPL) Version 3
  * 
@@ -41,7 +41,8 @@ static uint8_t debug_num_channels;
 /**
 * Initialise Debug-features
 */
-void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels)
+void PIOS_DEBUG_Init(__attribute__((unused)) const struct pios_tim_channel * channels,
+						__attribute__((unused)) uint8_t num_channels)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
 	PIOS_Assert(channels);
@@ -60,13 +61,13 @@ void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_chann
 		GPIO_StructInit(&GPIO_InitStructure);
 		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
 		GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-		GPIO_InitStructure.GPIO_Pin = chan->init->GPIO_Pin;
+		GPIO_InitStructure.GPIO_Pin = chan->pin.init.GPIO_Pin;
 
 		/* Initialize the GPIO */
-		GPIO_Init(chan->init->port, &GPIO_InitStructure);
+		GPIO_Init(chan->pin.gpio, &GPIO_InitStructure);
 
 		/* Set the pin low */
-		GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
+		GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET);
 	}
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
@@ -75,7 +76,7 @@ void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_chann
 * Set debug-pin high
 * \param pin 0 for S1 output
 */
-void PIOS_DEBUG_PinHigh(uint8_t pin)
+void PIOS_DEBUG_PinHigh(__attribute__((unused)) uint8_t pin)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
 	if (!debug_channels || pin >= debug_num_channels) {
@@ -84,7 +85,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin)
 
 	const struct pios_tim_channel * chan = &debug_channels[pin];
 
-	GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_Set);
+	GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_SET);
 
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
@@ -93,7 +94,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin)
 * Set debug-pin low
 * \param pin 0 for S1 output
 */
-void PIOS_DEBUG_PinLow(uint8_t pin)
+void PIOS_DEBUG_PinLow(__attribute__((unused)) uint8_t pin)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
 	if (!debug_channels || pin >= debug_num_channels) {
@@ -102,13 +103,13 @@ void PIOS_DEBUG_PinLow(uint8_t pin)
 
 	const struct pios_tim_channel * chan = &debug_channels[pin];
 
-	GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
+	GPIO_WriteBit(chan->pin.gpio, chan->pin.init.GPIO_Pin, Bit_RESET);
 
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
 
 
-void PIOS_DEBUG_PinValue8Bit(uint8_t value)
+void PIOS_DEBUG_PinValue8Bit(__attribute__((unused)) uint8_t value)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
 	if (!debug_channels) {
@@ -124,14 +125,14 @@ void PIOS_DEBUG_PinValue8Bit(uint8_t value)
 	 * This is sketchy since it assumes a particular ordering
 	 * and bitwise layout of the channels provided to the debug code.
 	 */
-	debug_channels[0].init.port->BSRR = bsrr_l;
+	debug_channels[0].pin.gpio->BSRR = bsrr_l;
-	debug_channels[4].init.port->BSRR = bsrr_h;
+	debug_channels[4].pin.gpio->BSRR = bsrr_h;
 
 	PIOS_IRQ_Enable();
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
 
-void PIOS_DEBUG_PinValue4BitL(uint8_t value)
+void PIOS_DEBUG_PinValue4BitL(__attribute__((unused)) uint8_t value)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
 	if (!debug_channels) {
@@ -143,7 +144,7 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value)
 	 * and bitwise layout of the channels provided to the debug code.
 	 */
 	uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6);
-	debug_channels[0].init.port->BSRR = bsrr_l;
+	debug_channels[0].pin.gpio->BSRR = bsrr_l;
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
 
@@ -151,7 +152,7 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value)
 /**
  * Report a serious error and halt
  */
-void PIOS_DEBUG_Panic(const char *msg)
+void PIOS_DEBUG_Panic(__attribute__((unused)) const char *msg)
 {
 #ifdef PIOS_INCLUDE_DEBUG_CONSOLE
 	register int *lr asm("lr");	// Link-register holds the PC of the caller

flight/pios/stm32f10x/pios_eeprom.c

@@ -88,7 +88,7 @@ int32_t PIOS_EEPROM_Save(uint8_t *data, uint32_t len)
 	// write 4 bytes at a time into program flash area (emulated EEPROM area)
 	uint8_t *p1 = data;
 	uint32_t *p3 = (uint32_t *)config.base_address;
-	for (int32_t i = 0; i < size; p3++)
+	for (uint32_t i = 0; i < size; p3++)
 	{
 		uint32_t value = 0;
 

flight/pios/stm32f10x/pios_led.c

@@ -34,7 +34,7 @@
 
 #include <pios_led_priv.h>
 
-const static struct pios_led_cfg * led_cfg;
+static const struct pios_led_cfg * led_cfg;
 
 /**
 * Initialises all the LED's

flight/pios/stm32f10x/pios_ppm.c

@@ -114,7 +114,7 @@ static struct pios_ppm_dev * PIOS_PPM_alloc(void)
 
 static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
 static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
-const static struct pios_tim_callbacks tim_callbacks = {
+static const struct pios_tim_callbacks tim_callbacks = {
 	.overflow = PIOS_PPM_tim_overflow_cb,
 	.edge     = PIOS_PPM_tim_edge_cb,
 };
@@ -188,6 +188,7 @@ extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 	TIM_ICInitStructure.TIM_ICFilter = 0x0;
 
+	ppm_dev->supv_timer = 0;
 	if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
 		PIOS_DEBUG_Assert(0);
 	}
@@ -223,7 +224,10 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
 	return ppm_dev->CaptureValue[channel];
 }
 
-static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
+static void PIOS_PPM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id,
+									   uint32_t context,
+									   __attribute__((unused)) uint8_t channel,
+									   uint16_t count)
 {
 	struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
 
@@ -238,7 +242,10 @@ static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
 }
 
 
-static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
+static void PIOS_PPM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id,
+								   uint32_t context,
+								   uint8_t chan_idx,
+								   uint16_t count)
 {
 	/* Recover our device context */
 	struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
@@ -286,7 +293,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha
 		/* Check if the last frame was well formed */
 		if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) {
 			/* The last frame was well formed */
-			for (uint32_t i = 0; i < ppm_dev->NumChannels; i++) {
+			for (int32_t i = 0; i < ppm_dev->NumChannels; i++) {
 				ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i];
 			}
 			for (uint32_t i = ppm_dev->NumChannels;

flight/pios/stm32f10x/pios_ppm_out.c

@@ -49,16 +49,20 @@ struct pios_ppm_out_dev {
 	enum pios_ppm_out_dev_magic magic;
 	const struct pios_ppm_out_cfg * cfg;
 
-	uint32_t triggering_period;
+	uint32_t TriggeringPeriod;
 	uint32_t ChannelSum;
 	uint8_t NumChannelCounter;
 	uint16_t ChannelValue[PIOS_PPM_OUT_MAX_CHANNELS];
 
-	uint8_t supv_timer;
+	uint8_t SupvTimer;
-	bool Tracking;
 	bool Fresh;
+	bool Tracking;
+        bool Enabled;
 };
 
+static void PIOS_PPM_Out_Supervisor(uint32_t ppm_id);
+static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable);
+
 static bool PIOS_PPM_Out_validate(struct pios_ppm_out_dev *ppm_dev)
 {
 	return (ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC);
@@ -94,7 +98,7 @@ static struct pios_ppm_out_dev * PIOS_PPM_alloc(void)
 #endif
 
 static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count);
-const static struct pios_tim_callbacks tim_out_callbacks = {
+static const struct pios_tim_callbacks tim_out_callbacks = {
 	.overflow = NULL,
 	.edge     = PIOS_PPM_OUT_tim_edge_cb,
 };
@@ -115,12 +119,12 @@ int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *
 	ppm_dev->cfg = cfg;
 
 	// Set up the state variables
-	ppm_dev->triggering_period = PIOS_PPM_OUT_HIGH_PULSE_US;
+	ppm_dev->TriggeringPeriod = PIOS_PPM_OUT_HIGH_PULSE_US;
 	ppm_dev->ChannelSum = 0;
 	ppm_dev->NumChannelCounter = 0;
 
 	// Flush counter variables
-	for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; i++)
+	for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i)
 		ppm_dev->ChannelValue[i] = 1000;
 
 	uint32_t tim_id;
@@ -149,20 +153,6 @@ int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *
 		TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 		break;
 	}
-	switch (chan->timer_chan) {
-	case TIM_Channel_1:
-		TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE);
-		break;
-	case TIM_Channel_2:
-		TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE);
-		break;
-	case TIM_Channel_3:
-		TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE);
-		break;
-	case TIM_Channel_4:
-		TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE);
-		break;
-	}
 
 	TIM_ARRPreloadConfig(chan->timer, ENABLE);
 	TIM_CtrlPWMOutputs(chan->timer, ENABLE);
@@ -174,20 +164,17 @@ int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *
 	TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
 	TIM_TimeBaseStructure.TIM_Period = ((1000000 / 100) - 1);
 	TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
-	switch(chan->timer_chan) {
+        PIOS_PPM_Out_Enable_Disable(ppm_dev, false);
-	case TIM_Channel_1:
+
-		TIM_SetCompare1(chan->timer, ppm_dev->triggering_period);
+        // Configure the supervisor
-		break;
+	ppm_dev->SupvTimer = 0;
-	case TIM_Channel_2:
+	ppm_dev->Fresh = FALSE;
-		TIM_SetCompare2(chan->timer, ppm_dev->triggering_period);
+	ppm_dev->Tracking = FALSE;
-		break;
+        ppm_dev->Enabled = FALSE;
-	case TIM_Channel_3:
+	if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) {
-		TIM_SetCompare3(chan->timer, ppm_dev->triggering_period);
+		PIOS_DEBUG_Assert(0);
-		break;
-	case TIM_Channel_4:
-		TIM_SetCompare4(chan->timer, ppm_dev->triggering_period);
-		break;
 	}
+
 	return 0;
 }
 
@@ -202,24 +189,52 @@ void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position)
 		position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US;
 	if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US)
 		position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US;
+
+        // Update the supervisor tracking variables.
+	ppm_dev->Fresh = TRUE;
+
+        // Reenable the TIM if it's been turned off.
+        if (!ppm_dev->Tracking) {
+                ppm_dev->Tracking = TRUE;
+                PIOS_PPM_Out_Enable_Disable(ppm_dev, true);
+        }
 	
 	// Update the position
 	ppm_dev->ChannelValue[servo] = position;
 }
 
-static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
+static void PIOS_PPM_OUT_tim_edge_cb (__attribute__((unused)) uint32_t tim_id,
+										uint32_t context,
+										__attribute__((unused)) uint8_t chan_idx,
+										__attribute__((unused)) uint16_t count)
 {
 	struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context;
 	if (!PIOS_PPM_Out_validate(ppm_dev))
 		return;
 
+        // Just return if the device is disabled.
+        if (!ppm_dev->Enabled) {
+                return;
+        }
+
+        // Turn off the PPM stream if we are no longer receiving update
+        // Note: This must happen between frames.
+        if ((ppm_dev->NumChannelCounter == 0) && !ppm_dev->Tracking) {
+                // Flush counter variables
+                for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) {
+                        ppm_dev->ChannelValue[i] = 1000;
+                }
+                PIOS_PPM_Out_Enable_Disable(ppm_dev, false);
+                return;
+        }
+
 	// Finish out the frame if we reached the last channel.
 	uint32_t pulse_width;
 	if ((ppm_dev->NumChannelCounter >= PIOS_PPM_OUT_MAX_CHANNELS)) {
 		pulse_width = PIOS_PPM_OUT_FRAME_PERIOD_US - ppm_dev->ChannelSum;
 		ppm_dev->NumChannelCounter = 0;
 		ppm_dev->ChannelSum = 0;
-	} else
+        } else
 		ppm_dev->ChannelSum += (pulse_width = ppm_dev->ChannelValue[ppm_dev->NumChannelCounter++]);
 
 	// Initiate the pulse
@@ -228,4 +243,50 @@ static void PIOS_PPM_OUT_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t
 	return;
 }
 
+static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable)
+{
+	const struct pios_tim_channel *chan = ppm_dev->cfg->channel;
+        uint32_t trig = enable ? ppm_dev->TriggeringPeriod : 0;
+        FunctionalState state = enable ? ENABLE : DISABLE;
+        ppm_dev->Enabled = enable;
+        switch (chan->timer_chan) {
+        case TIM_Channel_1:
+                TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, state);
+                TIM_SetCompare1(chan->timer, trig);
+                break;
+        case TIM_Channel_2:
+                TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, state);
+                TIM_SetCompare2(chan->timer, trig);
+                break;
+        case TIM_Channel_3:
+                TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, state);
+                TIM_SetCompare3(chan->timer, trig);
+                break;
+        case TIM_Channel_4:
+                TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, state);
+                TIM_SetCompare4(chan->timer, trig);
+                break;
+        }
+}
+
+static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id) {
+	struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id;
+	if (!PIOS_PPM_Out_validate(ppm_dev))
+		return;
+
+        // RTC runs at 625Hz so divide down the base rate so that this loop runs at 12.5Hz.
+	if(++(ppm_dev->SupvTimer) < 50) {
+		return;
+	}
+	ppm_dev->SupvTimer = 0;
+
+        // Go into failsafe the channel values haven't been refreshed since the last time through.
+	if (!ppm_dev->Fresh) {
+		ppm_dev->Tracking = FALSE;
+	}
+
+        // Set Fresh to false to test if channel values are being refreshed.
+	ppm_dev->Fresh = FALSE;
+}
+
 #endif /* PIOS_INCLUDE_PPM_OUT */

flight/pios/stm32f10x/pios_pwm.c

@@ -44,7 +44,7 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
 
 /* Local Variables */
 /* 100 ms timeout without updates on channels */
-const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
+static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
 
 enum pios_pwm_dev_magic {
 	PIOS_PWM_DEV_MAGIC = 0xab30293c,
@@ -98,7 +98,7 @@ static struct pios_pwm_dev * PIOS_PWM_alloc(void)
 
 static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
 static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
-const static struct pios_tim_callbacks tim_callbacks = {
+static const struct pios_tim_callbacks tim_callbacks = {
 	.overflow = PIOS_PWM_tim_overflow_cb,
 	.edge     = PIOS_PWM_tim_edge_cb,
 };
@@ -193,7 +193,7 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel)
 	return pwm_dev->CaptureValue[channel];
 }
 
-static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
+static void PIOS_PWM_tim_overflow_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
 {
 	struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;
 
@@ -219,7 +219,7 @@ static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t
 	return;
 }
 
-static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
+static void PIOS_PWM_tim_edge_cb (__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
 {
 	/* Recover our device context */
 	struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;

flight/pios/stm32f10x/pios_spi.c

@@ -39,7 +39,7 @@
 
 #include <pios_spi_priv.h>
 
-static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev)
+static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev * com_dev)
 {
 	/* Should check device magic here */
 	return(true);

flight/pios/stm32f10x/pios_tim.c

@@ -134,7 +134,7 @@ int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel *
 			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
 			break;
 		case (uint32_t) GPIOC:
-			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
 			break;
 		default:
 			PIOS_Assert(0);

flight/pios/stm32f10x/pios_usart.c

@@ -196,7 +196,7 @@ out_fail:
 	return(-1);
 }
 
-static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
+static void PIOS_USART_RxStart(uint32_t usart_id, __attribute__((unused)) uint16_t rx_bytes_avail)
 {
 	struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
 
@@ -205,7 +205,7 @@ static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail)
 
 	USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE);
 }
-static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail)
+static void PIOS_USART_TxStart(uint32_t usart_id, __attribute__((unused)) uint16_t tx_bytes_avail)
 {
 	struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id;
 

flight/pios/stm32f10x/pios_usb.c

@@ -208,7 +208,7 @@ int32_t PIOS_USB_Reenumerate()
 	return 0;
 }
 
-bool PIOS_USB_CableConnected(uint8_t id)
+bool PIOS_USB_CableConnected(__attribute__((unused)) uint8_t id)
 {
 	struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id;
 

flight/pios/stm32f10x/pios_usb_cdc.c

@@ -229,7 +229,7 @@ static void PIOS_USB_CDC_SendData(struct pios_usb_cdc_dev * usb_cdc_dev)
 #endif	/* PIOS_INCLUDE_FREERTOS */
 }
 
-static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, uint16_t tx_bytes_avail)
+static void PIOS_USB_CDC_TxStart(uint32_t usbcdc_id, __attribute__((unused)) uint16_t tx_bytes_avail)
 {
 	struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)usbcdc_id;
 

flight/pios/stm32f10x/pios_usb_hid.c

@@ -223,7 +223,7 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) {
 	PIOS_IRQ_Enable();
 }
 
-static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, uint16_t tx_bytes_avail)
+static void PIOS_USB_HID_TxStart(uint32_t usbhid_id, __attribute__((unused)) uint16_t tx_bytes_avail)
 {
 	struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbhid_id;
 

flight/pios/stm32f10x/pios_usb_hid_pwr.c

@@ -44,7 +44,7 @@ struct {
  * Output         : None.
  * Return         : None
  *******************************************************************************/
-void USB_Cable_Config(FunctionalState NewState)
+void USB_Cable_Config(__attribute__((unused)) FunctionalState NewState)
 {
 }