Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into OP-932-Modularize_Radio_Driver

Makefile

@@ -914,32 +914,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"
@@ -948,14 +948,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]"
@@ -966,22 +966,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) "         All tools will be installed into $(TOOLS_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,8 +81,10 @@ 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;
@@ -84,40 +95,198 @@ 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);
-		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();
 		myLocationOverlay.enableCompass();
 		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
 	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);
 		Bitmap uavSymbol = BitmapFactory.decodeResource(getResources(), R.drawable.ic_uav);

flight/Project/gdb/osd

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

flight/libraries/paths.c

@@ -32,8 +32,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,7 +46,7 @@ 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_DRIVEVECTOR:
             return path_vector(start_point, end_point, cur_point, status);
@@ -75,7 +75,7 @@ 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 dist_path, dist_diff;
@@ -91,8 +91,8 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
     diff_north = end_point[0] - cur_point[0];
     diff_east = end_point[1] - cur_point[1];
 
-	dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
+    dist_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;
@@ -117,7 +117,7 @@ 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 dist_path;
@@ -133,13 +133,13 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
     diff_east = cur_point[1] - start_point[1];
 
     dot = path_north * diff_north + path_east * diff_east;
-	dist_path = sqrtf( path_north * path_north + path_east * path_east );
+    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.
         // Fly towards the endpoint to prevent flying away,
         // 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;
         return;
     }
@@ -174,8 +174,10 @@ 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,cradius;
+    float radius, cradius;
     float normal[2];
+    float progress;
+    float a_diff, a_radius;
 
     // Radius
     radius_north = end_point[0] - start_point[0];
@@ -185,8 +187,8 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
     diff_north = cur_point[0] - end_point[0];
     diff_east = cur_point[1] - end_point[1];
 
-	radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
+    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
@@ -209,14 +211,37 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
         normal[1] = -diff_north / cradius;
     }
 
-	status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east);
+    // normalize progress to 0..1
+    a_diff = atan2f(diff_north, diff_east);
+    a_radius = atan2f(radius_north, radius_east);
+
+    if (a_diff < 0) {
+        a_diff += 2 * M_PI;
+    }
+    if (a_radius < 0) {
+        a_radius += 2 * M_PI;
+    }
+
+    progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
+
+    if (progress < 0) {
+        progress += 1.0;
+    } else if (progress >= 1) {
+        progress -= 1.0;
+    }
+
+    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[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius;
-	status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
+    status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
 
     // Compute direction to travel
     status->path_direction[0] = normal[0];

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;
@@ -110,21 +111,56 @@ int32_t configuration_check()
             case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
                 if (coptercontrol) {
                     severity = SYSTEMALARMS_ALARM_ERROR;
-                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold
+                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) {
+                    // Revo supports altitude hold
                     severity = SYSTEMALARMS_ALARM_ERROR;
                 }
                 break;
             case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
                 if (coptercontrol) {
                     severity = SYSTEMALARMS_ALARM_ERROR;
-                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
+                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports VelocityControl
                     severity = SYSTEMALARMS_ALARM_ERROR;
                 }
                 break;
             case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
-                if (coptercontrol){
+                if (coptercontrol) {
                     severity = SYSTEMALARMS_ALARM_ERROR;
-                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
+                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports Position Hold
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports AutoLand Mode
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports POI Mode
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports PathPlanner
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
+                break;
+            case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
+                if (coptercontrol) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                } else if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+                    // Revo supports ReturnToBase
                     severity = SYSTEMALARMS_ALARM_ERROR;
                 }
                 break;
@@ -141,7 +177,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);
@@ -158,14 +194,14 @@ int32_t configuration_check()
 static int32_t check_stabilization_settings(int index, bool multirotor)
 {
     // Make sure the modes have identical sizes
-    if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
+    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 +217,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/Altitude/altitude.c

@@ -39,7 +39,9 @@
 #include "openpilot.h"
 #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
@@ -55,12 +57,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);
@@ -73,9 +78,11 @@ int32_t AltitudeStart()
 {
 
 	if (altitudeEnabled) {
+#if defined(PIOS_INCLUDE_BMP085)
 		BaroAltitudeInitialize();
+#endif
 #if defined(PIOS_INCLUDE_HCSR04)
-		SonarAltitudeInitialze();
+		SonarAltitudeInitialize();
 #endif
 
 		// Start main task
@@ -105,11 +112,13 @@ 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)
@@ -118,46 +127,50 @@ MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
  */
 static void altitudeTask(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();
+	}
 #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();
-			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);
 					sonardata.Altitude = height_out; // m/us
 				}
 
 				// Update the AltitudeActual UAVObject
 				SonarAltitudeSet(&sonardata);
-			timeout=5;
+				timeout = 5;
 				PIOS_HCSR04_Trigger();
 			}
-		if(timeout--)
+			if(!(timeout--)) {
-		{
 				//retrigger
-			timeout=5;
+				timeout = 5;
 				PIOS_HCSR04_Trigger();
 			}
+		}
 #endif
+#if defined(PIOS_INCLUDE_BMP085)
 		// Update the temperature data
 		PIOS_BMP085_StartADC(TemperatureConv);
 #ifdef PIOS_BMP085_HAS_GPIOS
@@ -196,6 +209,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

@@ -61,10 +61,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);
 	TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);
@@ -79,7 +75,9 @@ int32_t AltitudeStart()
 int32_t AltitudeInitialize()
 {
 	BaroAltitudeInitialize();
-
+#if defined(PIOS_INCLUDE_HCSR04)
+	SonarAltitudeInitialize();
+#endif
 	return 0;
 }
 MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
@@ -92,9 +90,8 @@ static void altitudeTask(void *parameters)
 	
 #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
 
@@ -111,28 +108,30 @@ 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--)) {
+			if(PIOS_HCSR04_Completed()) {
 				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);
 					sonardata.Altitude = height_out; // m/us
 				}
 
-			// Update the AltitudeActual UAVObject
+				// Update the SonarAltitude UAVObject
 				SonarAltitudeSet(&sonardata);
-			timeout=5;
+				timeout = 10;
 				PIOS_HCSR04_Trigger();
 			}
-		if(timeout--)
+			if(!(timeout--)) {
-		{
 				//retrigger
-			timeout=5;
+				timeout = 10;
 				PIOS_HCSR04_Trigger();
 			}
+			sample_rate = 25;
+		}
 #endif
 		float temp, press;
 #ifdef PIOS_MS5611_SLOW_TEMP_RATE

flight/modules/Attitude/revolution/attitude.c

@@ -587,8 +587,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;
+
+	// 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);

flight/modules/Extensions/MagBaro/magbaro.c

@@ -43,9 +43,9 @@
 #include "magnetometer.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
 
@@ -53,14 +53,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);
@@ -75,7 +81,7 @@ int32_t MagBaroStart()
 	if (magbaroEnabled) {
 		// Start main task
 		xTaskCreate(magbaroTask, (signed char *)"MagBaro", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
-		//TaskMonitorAdd(TASKINFO_RUNNING_MAGBARO, taskHandle);
+		TaskMonitorAdd(TASKINFO_RUNNING_MAGBARO, taskHandle);
 		return 0;
 	}
 	return -1;
@@ -102,13 +108,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;
 }
@@ -116,7 +127,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,
@@ -127,27 +138,25 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
 	.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
 
 };
+#endif
 
 static void magbaroTask(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)
@@ -192,7 +201,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/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

@@ -73,11 +73,7 @@
 // Private types
 typedef enum
 {
-	ARM_STATE_DISARMED,
+    ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT
-	ARM_STATE_ARMING_MANUAL,
-	ARM_STATE_ARMED,
-	ARM_STATE_DISARMING_MANUAL,
-	ARM_STATE_DISARMING_TIMEOUT
 } ArmState_t;
 
 // Private variables
@@ -93,6 +89,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);
@@ -113,7 +110,8 @@ 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];
     uint8_t sample_count;
@@ -131,7 +129,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
 int32_t ManualControlStart()
 {
     // 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);
     TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
     PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
 
@@ -145,7 +143,7 @@ int32_t ManualControlInitialize()
 {
 
     /* Check the assumptions about uavobject enum's are correct */
-	if(!assumptions)
+    if (!assumptions)
         return -1;
 
     AccessoryDesiredInitialize();
@@ -157,7 +155,7 @@ int32_t ManualControlInitialize()
 
     return 0;
 }
-MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
+MODULE_INITCALL( ManualControlInitialize, ManualControlStart)
 
 /**
  * Module task
@@ -223,7 +221,7 @@ static void manualControlTask(void *parameters)
         if (ManualControlCommandReadOnly()) {
             FlightTelemetryStatsData flightTelemStats;
             FlightTelemetryStatsGet(&flightTelemStats);
-			if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
+            if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
                 /* trying to fly via GCS and lost connection.  fall back to transmitter */
                 UAVObjMetadata metadata;
                 ManualControlCommandGetMetadata(&metadata);
@@ -237,48 +235,47 @@ static void manualControlTask(void *parameters)
             bool valid_input_detected = true;
 
             // Read channel values in us
-			for (uint8_t n = 0; 
+            for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
-			     n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
-			     ++n) {
                 extern uint32_t pios_rcvr_group_map[];
 
                 if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
                     cmd.Channel[n] = PIOS_RCVR_INVALID;
                 } else {
-					cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
+                    cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[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)
+                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
-			if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
+            if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
+                    || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
+                    || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
-				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= 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_ROLL] == (uint16_t) PIOS_RCVR_INVALID
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (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_YAW] == (uint16_t) PIOS_RCVR_INVALID
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (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_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (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_YAW] == (uint16_t) PIOS_RCVR_NODRIVER
-				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (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.FlightModeNumber > 1)
-					settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
+                            && (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_INVALID
-					cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
+                                    || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
 
                 AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
                 cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
@@ -292,10 +289,14 @@ static void manualControlTask(void *parameters)
             }
 
             // decide if we have valid manual input or not
-			valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) &&
+            valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
-			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) &&
+                    settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
-			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) &&
+                    && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
-			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
+                            settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
+                    && 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]);
 
             // Implement hysteresis loop on connection status
             if (valid_input_detected && (++connected_count > 10)) {
@@ -321,24 +322,21 @@ static void manualControlTask(void *parameters)
 
                 AccessoryDesiredData accessory;
                 // Set Accessory 0
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != 
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
                     accessory.AccessoryVal = 0;
-					if(AccessoryDesiredInstSet(0, &accessory) != 0)
+                    if (AccessoryDesiredInstSet(0, &accessory) != 0)
                         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
                 }
                 // Set Accessory 1
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != 
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
                     accessory.AccessoryVal = 0;
-					if(AccessoryDesiredInstSet(1, &accessory) != 0)
+                    if (AccessoryDesiredInstSet(1, &accessory) != 0)
                         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
                 }
                 // Set Accessory 2
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != 
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
                     accessory.AccessoryVal = 0;
-					if(AccessoryDesiredInstSet(2, &accessory) != 0)
+                    if (AccessoryDesiredInstSet(2, &accessory) != 0)
                         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
                 }
 
@@ -370,40 +368,37 @@ static void manualControlTask(void *parameters)
                 applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &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];
 
                 AccessoryDesiredData accessory;
                 // Set Accessory 0
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != 
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					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)
+                    if (AccessoryDesiredInstSet(0, &accessory) != 0)
                         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
                 }
                 // Set Accessory 1
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != 
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					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)
+                    if (AccessoryDesiredInstSet(1, &accessory) != 0)
                         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
                 }
                 // Set Accessory 2
-				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != 
+                if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
-					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)
+                    if (AccessoryDesiredInstSet(2, &accessory) != 0)
                         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
                 }
 
@@ -434,7 +429,7 @@ static void manualControlTask(void *parameters)
 
         // Depending on the mode update the Stabilization or Actuator objects
         static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
-		switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
+        switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
             case FLIGHTMODE_UNDEFINED:
                 // This reflects a bug in the code architecture!
                 AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@@ -450,16 +445,23 @@ static void manualControlTask(void *parameters)
                 // call anything else.  This just avoids errors.
                 break;
             case FLIGHTMODE_GUIDANCE:
-				switch(flightStatus.FlightMode) {
+                switch (flightStatus.FlightMode) {
                     case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
                         altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
                         break;
                     case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+                    case FLIGHTSTATUS_FLIGHTMODE_POI:
                         updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
                         break;
                     case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
                         updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
                         break;
+                    case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+                        // No need to call anything.  This just avoids errors.
+                        break;
+                    case FLIGHTSTATUS_FLIGHTMODE_LAND:
+                        updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
+                        break;
                     default:
                         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
                 }
@@ -476,8 +478,7 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
 
     /* Clear all channel activity flags */
     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.ActiveChannel = 255;
         updated = true;
@@ -491,7 +492,8 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
     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
         samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
@@ -503,9 +505,7 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
     bool activity_updated = false;
 
     /* 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;
-	     channel++) {
         uint16_t delta;
         uint16_t prev = fsm->prev[channel - 1];   // Subtract 1 because channels are 1 indexed
         uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
@@ -547,7 +547,7 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
                     break;
             }
 
-			ReceiverActivityActiveGroupSet((uint8_t*)&group);
+            ReceiverActivityActiveGroupSet((uint8_t*) &group);
             ReceiverActivityActiveChannelSet(&channel);
             activity_updated = true;
         }
@@ -572,9 +572,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
 
     if (fsm->sample_count == 0) {
         /* 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,
-					NELEMENTS(fsm->prev));
         fsm->sample_count++;
         return (false);
     }
@@ -582,7 +580,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
     /* Compare with previous sample */
     activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
 
-group_completed:
+    group_completed:
     /* Reset the sample counter */
     fsm->sample_count = 0;
 
@@ -600,9 +598,7 @@ group_completed:
              * extra 20ms delay in the main thread so we can speed up
              * this algorithm.
              */
-			updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
+            updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
-						fsm->prev,
-						NELEMENTS(fsm->prev));
             fsm->sample_count++;
             break;
         }
@@ -633,7 +629,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
     uint8_t * stab_settings;
     FlightStatusData flightStatus;
     FlightStatusGet(&flightStatus);
-	switch(flightStatus.FlightMode) {
+    switch (flightStatus.FlightMode) {
         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
             stab_settings = settings->Stabilization1Settings;
             break;
@@ -654,35 +650,41 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
     stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
     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_NONE) ? cmd->Roll :
             (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
+                    cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
             (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
             (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
             (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
-	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
+                    cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
-	     0; // this is an invalid mode
+            (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
     ;
-	stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
+    stabilization.Pitch =
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
             (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] :
+            (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
+                    cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
             (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+            (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_RELAYRATE) ?
-	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
+                    cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
-	     0; // this is an invalid mode
+            (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_NONE) ? cmd->Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
+                    cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
-	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
-	     0; // this is an invalid mode
 
     stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
     StabilizationDesiredSet(&stabilization);
@@ -725,12 +727,12 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
 
         PathDesiredData pathDesired;
         PathDesiredGet(&pathDesired);
-		pathDesired.Start[PATHDESIRED_END_NORTH] = positionActual.North;
+        pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
-		pathDesired.Start[PATHDESIRED_END_EAST] = positionActual.East;
+        pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
-		pathDesired.Start[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
+        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 - 10;
+        pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
         pathDesired.StartingVelocity=1;
         pathDesired.EndingVelocity=0;
         pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@@ -747,6 +749,37 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
     }
 }
 
+static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
+{
+    static portTickType lastSysTime;
+    portTickType thisSysTime;
+    float dT;
+
+    thisSysTime = xTaskGetTickCount();
+    dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
+    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);
+}
+
 /**
  * @brief Update the altitude desired to current altitude when
  * enabled and enable altitude mode for stabilization
@@ -781,12 +814,14 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
         // After not being in this mode for a while init at current height
         altitudeHoldDesired.Altitude = 0;
         zeroed = false;
-	} else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
+    } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
         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;
-	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) {
+        // Require the stick to enter the dead band before they can move height
         zeroed = true;
+    }
 
     AltitudeHoldDesiredSet(&altitudeHoldDesired);
 }
@@ -800,6 +835,11 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool
     AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
 }
 
+static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
+{
+    AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
+}
+
 static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
 {
     AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
@@ -813,33 +853,36 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
     float valueScaled;
 
     // 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;
         }
-	else
+    } else {
-	{
+        if (min != neutral) {
-		if (min != neutral)
+            valueScaled = (float) (value - neutral) / (float) (neutral - min);
-			valueScaled = (float)(value - neutral) / (float)(neutral - min);
+        } else {
-		else
             valueScaled = 0;
         }
+    }
 
     // Bound
-	if (valueScaled >  1.0) valueScaled =  1.0;
+    if (valueScaled > 1.0) {
-	else
+        valueScaled = 1.0;
-	if (valueScaled < -1.0) valueScaled = -1.0;
+    } else if (valueScaled < -1.0) {
+        valueScaled = -1.0;
+    }
 
     return valueScaled;
 }
 
-static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
+static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
-	if(end_time > start_time)
+{
+    if (end_time > start_time) {
         return (end_time - start_time) * portTICK_RATE_MS;
-	return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
+    }
+    return ((((portTICK_RATE_MS) - 1) - start_time) + end_time) * portTICK_RATE_MS;
 }
 
 /**
@@ -852,12 +895,9 @@ static bool okToArm(void)
     SystemAlarmsData alarms;
     SystemAlarmsGet(&alarms);
 
-
     // Check each alarm
-	for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
+    for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
-	{
+        if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) {	// found an alarm thats set
-		if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
-		{	// found an alarm thats set
             if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
                 continue;
 
@@ -887,11 +927,12 @@ static bool forcedDisArm(void)
  * @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);
 
-	if(flightStatus.Armed != val) {
+    if (flightStatus.Armed != val) {
         flightStatus.Armed = val;
         FlightStatusSet(&flightStatus);
     }
@@ -923,7 +964,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
 
         // The throttle is not low, in case we where arming or disarming, abort
         if (!lowThrottle) {
-			switch(armState) {
+            switch (armState) {
                 case ARM_STATE_DISARMING_MANUAL:
                 case ARM_STATE_DISARMING_TIMEOUT:
                     armState = ARM_STATE_ARMED;
@@ -945,18 +986,23 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
             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;
 
         // Calc channel see assumptions7
-		int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
+        int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
-		switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
+        switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
-			case ARMING_CHANNEL_ROLL:    armingInputLevel = sign * cmd->Roll;    break;
+            case ARMING_CHANNEL_ROLL:
-			case ARMING_CHANNEL_PITCH:   armingInputLevel = sign * cmd->Pitch;   break;
+                armingInputLevel = sign * cmd->Roll;
-			case ARMING_CHANNEL_YAW:     armingInputLevel = sign * cmd->Yaw;     break;
+                break;
+            case ARMING_CHANNEL_PITCH:
+                armingInputLevel = sign * cmd->Pitch;
+                break;
+            case ARMING_CHANNEL_YAW:
+                armingInputLevel = sign * cmd->Yaw;
+                break;
         }
 
         bool manualArm = false;
@@ -967,7 +1013,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
         else if (armingInputLevel >= +ARMED_THRESHOLD)
             manualDisarm = true;
 
-		switch(armState) {
+        switch (armState) {
             case ARM_STATE_DISARMED:
                 setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
 
@@ -1008,7 +1054,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
                 break;
 
             case ARM_STATE_DISARMING_MANUAL:
-				if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
+                if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
                     armState = ARM_STATE_DISARMED;
                 else if (!manualDisarm)
                     armState = ARM_STATE_ARMED;
@@ -1047,8 +1093,7 @@ 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;
         min = max;
         max = tmp;
@@ -1063,8 +1108,7 @@ static void applyDeadband(float *value, float deadband)
 {
     if (fabs(*value) < deadband)
         *value = 0.0f;
-	else
+    else if (*value > 0.0f)
-		if (*value > 0.0f)
         *value -= deadband;
     else
         *value += deadband;
@@ -1083,7 +1127,6 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
     }
 }
 #endif // USE_INPUT_LPF
-
 /**
  * Called whenever a critical configuration component changes
  */
@@ -1092,8 +1135,6 @@ static void configurationUpdatedCb(UAVObjEvent * ev)
     configuration_check();
 }
 
-
-
 /**
  * @}
  * @}

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
 
@@ -57,7 +55,7 @@ static uint32_t timeOfLastUpdateMs;
 int32_t WavPlayerStart(void)
 {
     // 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;
 }
@@ -71,11 +69,11 @@ int32_t WavPlayerInitialize(void)
 
     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)
@@ -83,9 +81,8 @@ static void WavPlayerTask(void *parameters)
     portTickType xDelay = 100 / portTICK_RATE_MS;
     portTickType lastSysTime;
     // 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;
     timeOfLastCommandMs = timeNowMs;
@@ -93,28 +90,12 @@ static void WavPlayerTask(void *parameters)
     WavePlayer_Start();
 #endif
     // Loop forever
-	while (1)
+    while (1) {
-	{
-
         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
@@ -98,7 +118,6 @@ struct FontDimensions
     int width, height;
 };
 
-
 // Max/Min macros.
 #define MAX(a, b)               ((a) > (b) ? (a) : (b))
 #define MIN(a, b)               ((a) < (b) ? (a) : (b))
@@ -123,7 +142,6 @@ struct FontDimensions
 // 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
  * @{
  *
@@ -29,7 +29,6 @@
  */
 
 // ****************
-
 #include "openpilot.h"
 #include "osdgen.h"
 #include "attitudeactual.h"
@@ -39,6 +38,7 @@
 #include "gpssatellites.h"
 #include "osdsettings.h"
 #include "baroaltitude.h"
+#include "flightstatus.h"
 
 #include "fonts.h"
 #include "font12x18.h"
@@ -47,29 +47,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;
 
 // ****************
@@ -99,88 +98,81 @@ struct splashEntry
     const uint16_t *mask;
 };
 
-struct splashEntry splash[3] = {
+struct splashEntry splash[3] =
-	{	oplogo_width,
+{
-		oplogo_height,
+{ oplogo_width, oplogo_height, oplogo_bits, oplogo_mask_bits },
-		oplogo_bits,
+{ level_width, level_height, level_bits, level_mask_bits },
-		oplogo_mask_bits },
+{ llama_width, llama_height, llama_bits, llama_mask_bits },
-	{	level_width,
-		level_height,
-		level_bits,
-		level_mask_bits },
-	{	llama_width,
-		llama_height,
-		llama_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;
 }
 
-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);
 }
 
-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)) {
         return;
     }
     struct splashEntry splash_info;
     splash_info = splash[image];
-	offsetx=offsetx/8;
+    offsetx = offsetx / 8;
-	for (uint16_t y = offsety; y < ((splash_info.height)+offsety); y++) {
+    for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) {
-		uint16_t x1=offsetx;
+        uint16_t x1 = offsetx;
-		for (uint16_t x = offsetx; x < (((splash_info.width)/16)+offsetx); x++) {
+        for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) {
-			draw_buffer_level[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8);
+            draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
-			draw_buffer_level[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
+                    mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
-			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_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)(
-			draw_buffer_mask[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
+                    mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
-			x1+=2;
+            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) {
+{
+    if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
         return 0;
     }
     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 ddF_y = -2 * 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_y == -2 * y;
         // f == x*x + y*y - radius*radius + 2*x - y + 1;
-	    if(f >= 0)
+        if (f >= 0) {
-	    {
             y--;
             ddF_y += 2;
             f += ddF_y;
@@ -188,32 +180,31 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) {
         x++;
         ddF_x += 2;
         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 - 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 + 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;
     *b = temp;
 }
 
+const static 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,
-  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};
-
-static int8_t mySin(uint16_t angle) {
     uint16_t pos = 0;
     pos = angle % 360;
     int8_t mult = 1;
@@ -229,7 +220,8 @@ static int8_t mySin(uint16_t angle) {
     return mult * (int8_t)(sinData[pos]);
 }
 
-static int8_t myCos(uint16_t angle) {
+static int8_t myCos(uint16_t angle)
+{
     return mySin(angle + 90);
 }
 
@@ -247,10 +239,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
@@ -275,15 +267,13 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
 
     plotFourQuadrants(centerX, centerY, x, y);
 
-    while(deltaY >= deltaX)
+    while (deltaY >= deltaX) {
-    {
         x++;
         deltaX += (doubleVerticalRadius << 1);
 
         error += deltaX + doubleVerticalRadius;
 
-          if(error >= 0)
+        if (error >= 0) {
-          {
             y--;
             deltaY -= (doubleHorizontalRadius << 1);
 
@@ -292,17 +282,16 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
         plotFourQuadrants(centerX, centerY, x, y);
     }
 
-    error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
+    error = (int64_t)(
+            doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
 
-    while (y>=0)
+    while (y >= 0) {
-    {
         error += doubleHorizontalRadius;
         y--;
-          deltaY -= (doubleHorizontalRadius<<1);
+        deltaY -= (doubleHorizontalRadius << 1);
         error -= deltaY;
 
-          if(error <= 0)
+        if (error <= 0) {
-          {
             x++;
             deltaX += (doubleVerticalRadius << 1);
             error += deltaX;
@@ -312,17 +301,16 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
     }
 }
 
-
 void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size)
 {
     int16_t a = myCos(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((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)
@@ -379,7 +367,6 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
     WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
 }
 
-
 /**
  * write_hline: optimised horizontal line writing algorithm
  *
@@ -393,11 +380,12 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
 {
     CLIP_COORDS(x0, y);
     CLIP_COORDS(x1, y);
-	if(x0 > x1)
+    if (x0 > x1) {
-	{
         SWAP(x0, x1);
     }
-	if(x0 == x1) return;
+    if (x0 == x1) {
+        return;
+    }
     /* This is an optimised algorithm for writing horizontal lines.
      * We begin by finding the addresses of the x0 and x1 points. */
     int addr0 = CALC_BUFF_ADDR(x0, y);
@@ -407,22 +395,18 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
     int mask, mask_l, mask_r, i;
     /* If the addresses are equal, we only need to write one word
      * which is an island. */
-	if(addr0 == addr1)
+    if (addr0 == addr1) {
-	{
         mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
         WRITE_WORD_MODE(buff, addr0, mask, mode);
-	}
+    } else {
         /* 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);
         WRITE_WORD_MODE(buff, addr0, mask_l, mode);
         WRITE_WORD_MODE(buff, addr1, mask_r, mode);
         // Now write 0xffff words from start+1 to end-1.
-		for(i = addr0 + 1; i <= addr1 - 1; i++)
+        for (i = addr0 + 1; i <= addr1 - 1; i++) {
-		{
+            uint8_t m = 0xff;
-			uint8_t m=0xff;
             WRITE_WORD_MODE(buff, i, m, mode);
         }
     }
@@ -461,8 +445,7 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int
 {
     int stroke, fill;
     SETUP_STROKE_FILL(stroke, fill, mode)
-	if(x0 > x1)
+    if (x0 > x1) {
-	{
         SWAP(x0, x1);
     }
     // Draw the main body of the line.
@@ -488,11 +471,12 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
     unsigned int a;
     CLIP_COORDS(x, y0);
     CLIP_COORDS(x, y1);
-	if(y0 > y1)
+    if (y0 > y1) {
-	{
         SWAP(y0, y1);
     }
-	if(y0 == y1) return;
+    if (y0 == y1) {
+        return;
+    }
     /* This is an optimised algorithm for writing vertical lines.
      * We begin by finding the addresses of the x,y0 and x,y1 points. */
     int addr0 = CALC_BUFF_ADDR(x, y0);
@@ -502,8 +486,7 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
     uint16_t mask = 1 << (7 - bitnum);
     /* Run from addr0 to addr1 placing pixels. Increment by the number
      * of words n each graphics line. */
-	for(a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8)
+    for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) {
-	{
         WRITE_WORD_MODE(buff, a, mask, mode);
     }
 }
@@ -540,8 +523,7 @@ 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;
-	if(y0 > y1)
+    if (y0 > y1) {
-	{
         SWAP(y0, y1);
     }
     SETUP_STROKE_FILL(stroke, fill, mode);
@@ -574,7 +556,9 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
     CHECK_COORDS(x, y);
     CHECK_COORD_X(x + width);
     CHECK_COORD_Y(y + height);
-        if(width <= 0 || height <= 0) return;
+    if (width <= 0 || height <= 0) {
+        return;
+    }
     // Calculate as if the rectangle was only a horizontal line. We then
     // step these addresses through each row until we iterate `height` times.
     int addr0 = CALC_BUFF_ADDR(x, y);
@@ -583,26 +567,21 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
     int addr1_bit = CALC_BIT_IN_WORD(x + width);
     int mask, mask_l, mask_r, i;
     // If the addresses are equal, we need to write one word vertically.
-        if(addr0 == addr1)
+    if (addr0 == addr1) {
-        {
         mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
-                while(height--)
+        while (height--) {
-                {
             WRITE_WORD_MODE(buff, addr0, mask, mode);
             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);
         // Write edges first.
         yy = 0;
         addr0_old = addr0;
         addr1_old = addr1;
-                while(yy < height)
+        while (yy < height) {
-                {
             WRITE_WORD_MODE(buff, addr0, mask_l, mode);
             WRITE_WORD_MODE(buff, addr1, mask_r, mode);
             addr0 += GRAPHICS_WIDTH_REAL / 8;
@@ -613,11 +592,9 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
         yy = 0;
         addr0 = addr0_old;
         addr1 = addr1_old;
-                while(yy < height)
+        while (yy < height) {
-                {
+            for (i = addr0 + 1; i <= addr1 - 1; i++) {
-                        for(i = addr0 + 1; i <= addr1 - 1; i++)
+                uint8_t m = 0xff;
-                        {
-                        	uint8_t m=0xff;
                 WRITE_WORD_MODE(buff, i, m, mode);
             }
             addr0 += GRAPHICS_WIDTH_REAL / 8;
@@ -681,16 +658,13 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int
 {
     CHECK_COORDS(cx, cy);
     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);
         }
         error += (y * 2) + 1;
         y++;
-		if(error >= 0)
+        if (error >= 0) {
-		{
             --x;
             error -= x * 2;
         }
@@ -716,10 +690,8 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
     // This is a two step procedure. First, we draw the outline of the
     // circle, then we draw the inner part.
     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, y + 1, mmode);
@@ -728,8 +700,7 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
             CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
             CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
             CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
-			if(bmode == 1)
+            if (bmode == 1) {
-			{
                 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_mask, cx, cy, x - 1, y - 1, mmode);
@@ -738,8 +709,7 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
         }
         error += (y * 2) + 1;
         y++;
-		if(error >= 0)
+        if (error >= 0) {
-		{
             --x;
             error -= x * 2;
         }
@@ -747,17 +717,14 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
     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, y, mmode);
             CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
         }
         error += (y * 2) + 1;
         y++;
-		if(error >= 0)
+        if (error >= 0) {
-		{
             --x;
             error -= x * 2;
         }
@@ -784,14 +751,11 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
     //
     // Due to multiple writes to each set of pixels, we have a special exception
     // 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);
-			if(mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y)))
+            if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) {
-			{
                 write_hline(buff, cx - y, cx + y, cy + x, mode);
                 write_hline(buff, cx - y, cx + y, cy - x, mode);
                 xch = 0;
@@ -799,16 +763,14 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
         }
         error += (y * 2) + 1;
         y++;
-		if(error >= 0)
+        if (error >= 0) {
-		{
             --x;
             xch = 1;
             error -= x * 2;
         }
     }
     // Handle toggle mode.
-	if(mode == 2)
+    if (mode == 2) {
-	{
         write_hline(buff, cx - r, cx + r, cy, mode);
     }
 }
@@ -827,13 +789,11 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
 {
     // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
     int steep = abs(y1 - y0) > abs(x1 - x0);
-	if(steep)
+    if (steep) {
-	{
         SWAP(x0, y0);
         SWAP(x1, y1);
     }
-	if(x0 > x1)
+    if (x0 > x1) {
-	{
         SWAP(x0, x1);
         SWAP(y0, y1);
     }
@@ -843,23 +803,19 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
     int ystep;
     int y = y0;
     int x; //, lasty = y, stox = 0;
-	if(y0 < y1)
+    if (y0 < y1) {
         ystep = 1;
-	else
+    } else {
         ystep = -1;
-	for(x = x0; x < x1; x++)
-	{
-		if(steep)
-		{
-			write_pixel(buff, y, x, mode);
     }
-		else
+    for (x = x0; x < x1; x++) {
-		{
+        if (steep) {
+            write_pixel(buff, y, x, mode);
+        } else {
             write_pixel(buff, x, y, mode);
         }
         error -= deltay;
-		if(error < 0)
+        if (error < 0) {
-		{
             y += ystep;
             error += deltax;
         }
@@ -900,24 +856,19 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
     // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
     // This could be improved for speed.
     int omode, imode;
-	if(mode == 0)
+    if (mode == 0) {
-	{
         omode = 0;
         imode = 1;
-	}
+    } else {
-	else
-	{
         omode = 1;
         imode = 0;
     }
     int steep = abs(y1 - y0) > abs(x1 - x0);
-	if(steep)
+    if (steep) {
-	{
         SWAP(x0, y0);
         SWAP(x1, y1);
     }
-	if(x0 > x1)
+    if (x0 > x1) {
-	{
         SWAP(x0, x1);
         SWAP(y0, y1);
     }
@@ -927,30 +878,26 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
     int ystep;
     int y = y0;
     int x;
-	if(y0 < y1)
+    if (y0 < y1) {
         ystep = 1;
-	else
+    } else {
         ystep = -1;
+    }
     // Draw the outline.
-	for(x = x0; x < x1; x++)
+    for (x = x0; x < x1; x++) {
-	{
+        if (steep) {
-		if(steep)
-		{
             write_pixel_lm(y - 1, x, mmode, omode);
             write_pixel_lm(y + 1, x, mmode, omode);
             write_pixel_lm(y, x - 1, mmode, omode);
             write_pixel_lm(y, x + 1, mmode, omode);
-		}
+        } else {
-		else
-		{
             write_pixel_lm(x - 1, y, mmode, omode);
             write_pixel_lm(x + 1, y, mmode, omode);
             write_pixel_lm(x, y - 1, mmode, omode);
             write_pixel_lm(x, y + 1, mmode, omode);
         }
         error -= deltay;
-		if(error < 0)
+        if (error < 0) {
-		{
             y += ystep;
             error += deltax;
         }
@@ -958,19 +905,14 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
     // Now draw the innards.
     error = deltax / 2;
     y = y0;
-	for(x = x0; x < x1; x++)
+    for (x = x0; x < x1; x++) {
-	{
+        if (steep) {
-		if(steep)
-		{
             write_pixel_lm(y, x, mmode, imode);
-		}
+        } else {
-		else
-		{
             write_pixel_lm(x, y, mmode, imode);
         }
         error -= deltay;
-		if(error < 0)
+        if (error < 0) {
-		{
             y += ystep;
             error += deltax;
         }
@@ -995,8 +937,9 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi
     uint16_t lastmask = word << (16 - xoff);
     WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, 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);
+    }
 }
 
 /**
@@ -1020,8 +963,9 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr,
     uint16_t lastmask = word << (16 - xoff);
     WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff);
     WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
-	if(xoff > 0)
+    if (xoff > 0) {
         WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8);
+    }
 }
 
 /**
@@ -1045,8 +989,9 @@ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, u
     uint16_t lastmask = word << (16 - xoff);
     WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff);
     WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
-	if(xoff > 0)
+    if (xoff > 0) {
         WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
+    }
 
 }
 
@@ -1077,21 +1022,21 @@ 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.
-	if(font > SIZEOF_ARRAY(fonts))
+    if (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];
     // Locate character in font lookup table. (If required.)
-	if(lookup != NULL)
+    if (lookup != NULL) {
-	{
         *lookup = font_info->lookup[ch];
-		if(*lookup == 0xff)
+        if (*lookup == 0xff) {
             return 0; // character doesn't exist, don't bother writing it.
         }
+    }
     return 1;
 }
 
-
 /**
  * write_char16: Draw a character on the current draw buffer.
  * Currently supports outlined characters and characters with
@@ -1120,20 +1065,21 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
     // wide for now. Support for large characters may be added in future.
     {
         // Ensure we don't overflow.
-		if(x + wbit > GRAPHICS_WIDTH_REAL)
+        if (x + wbit > GRAPHICS_WIDTH_REAL) {
             return;
+        }
         // Load data pointer.
         row = ch * font_info.height;
         row_temp = row;
         addr_temp = addr;
         xshift = 16 - font_info.width;
         // We can write mask words easily.
-		for(yy = y; yy < y + font_info.height; yy++)
+        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);
-			else
+            } else {
                 write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
+            }
             addr += GRAPHICS_WIDTH_REAL / 8;
             row++;
         }
@@ -1143,10 +1089,8 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
         // construct an AND mask and an OR mask, and apply each individually.
         row = row_temp;
         addr = addr_temp;
-		for(yy = y; yy < y + font_info.height; yy++)
+        for (yy = y; yy < y + font_info.height; yy++) {
-		{
+            if (font == 3) {
-			if(font==3)
-			{
                 level_bits = font_frame12x18[row];
                 //if(!(flags & FONT_INVERT)) // data is normally inverted
                 level_bits = ~level_bits;
@@ -1169,8 +1113,6 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
     }
 }
 
-
-
 /**
  * write_char: Draw a character on the current draw buffer.
  * Currently supports outlined characters and characters with
@@ -1201,19 +1143,18 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
     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.
-		if(x + wbit > GRAPHICS_WIDTH_REAL)
+        if (x + wbit > GRAPHICS_WIDTH_REAL) {
             return;
+        }
         // Load data pointer.
         row = lookup * font_info.height * 2;
         row_temp = row;
         addr_temp = addr;
         xshift = 16 - font_info.width;
         // 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);
             addr += GRAPHICS_WIDTH_REAL / 8;
             row++;
@@ -1224,11 +1165,12 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
         // construct an AND mask and an OR mask, and apply each individually.
         row = row_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)) // data is normally inverted
+            if (!(flags & FONT_INVERT)) {
+                // data is normally inverted
                 level_bits = ~level_bits;
+            }
             or_mask = font_info.data[row] << xshift;
             and_mask = (font_info.data[row] & level_bits) << xshift;
             write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
@@ -1242,51 +1184,51 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
 }
 
 /**
-* 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++;
-		if(*str == '\n' || *str == '\r')
+        if (*str == '\n' || *str == '\r') {
-		{
+            if (line_length > max_length) {
-			if(line_length > max_length)
                 max_length = line_length;
+            }
             line_length = 0;
             lines++;
         }
         str++;
     }
-    if(line_length > max_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;
@@ -1295,36 +1237,42 @@ 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;
-		}
+        } else {
-		else
+            if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) {
-		{
+                if (font_info.id < 2) {
-			if(xx >= 0 && xx < GRAPHICS_WIDTH_REAL)
-			{
-				if(font_info.id<2)
                     write_char(*str, xx, yy, flags, font);
-				else
+                } else {
                     write_char16(*str, xx, yy, font);
                 }
+            }
             xx += font_info.width + xs;
         }
         str++;
@@ -1332,18 +1280,18 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un
 }
 
 /**
-* 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)
 {
     int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
@@ -1360,53 +1308,51 @@ 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;
-            if(*str == '<' && fcode == 0) // begin format code?
+        }
-            {
+        if (*str == '<' && fcode == 0) {
+            // begin format code?
             fcode = 1;
             fptr = 0;
         }
-            if(*str == '>' && fcode == 1)
+        if (*str == '>' && fcode == 1) {
-            {
             fcode = 0;
-                    if(strcmp(fstack, "B")) // switch to "big" font (font #1)
+            if (strcmp(fstack, "B")) {
-                    {
+                // switch to "big" font (font #1)
                 fwidth = bigfontwidth;
                 fheight = bigfontheight;
-                    }
+            } else if (strcmp(fstack, "S")) {
-                    else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
+                // switch to "small" font (font #0)
-                    {
                 fwidth = smallfontwidth;
                 fheight = smallfontheight;
             }
-                    if(fheight > max_height)
+            if (fheight > max_height) {
                 max_height = fheight;
+            }
             // Skip over this byte. Go to next byte.
             str++;
             continue;
         }
-            if(*str != '<' && *str != '>' && fcode == 1)
+        if (*str != '<' && *str != '>' && fcode == 1) {
-            {
             // Add to the format stack (up to 10 bytes.)
-                    if(fptr > 10) // stop adding 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)
+        if (fcode == 0) {
-            {
             // Not a format code, raw text.
             xx += fwidth + xs;
-                    if(*str == '\n')
+            if (*str == '\n') {
-                    {
+                if (xx > max_xx) {
-                            if(xx > max_xx)
                     max_xx = xx;
+                }
                 xx = x;
                 yy += fheight + ys;
             }
@@ -1439,26 +1385,25 @@ 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;
-            if(*str == '<' && fcode == 0) // begin format code?
+        }
-            {
+        if (*str == '<' && fcode == 0) {
+            // begin format code?
             fcode = 1;
             fptr = 0;
         }
-            if(*str == '>' && fcode == 1)
+        if (*str == '>' && fcode == 1) {
-            {
             fcode = 0;
-                    if(strcmp(fstack, "B")) // switch to "big" font (font #1)
+            if (strcmp(fstack, "B")) {
-                    {
+                // switch to "big" font (font #1)
                 fwidth = bigfontwidth;
                 fheight = bigfontheight;
                 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;
                 fheight = smallfontheight;
                 font = 0;
@@ -1467,27 +1412,25 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
             str++;
             continue;
         }
-            if(*str != '<' && *str != '>' && fcode == 1)
+        if (*str != '<' && *str != '>' && fcode == 1) {
-            {
             // Add to the format stack (up to 10 bytes.)
-                    if(fptr > 10) // stop adding 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)
+        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 (*str == '\n') {
-                    {
+                if (xx > max_xx) {
-                            if(xx > max_xx)
                     max_xx = xx;
+                }
                 xx = x;
                 yy += fheight + ys;
             }
@@ -1496,86 +1439,81 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
     }
 }
 
-
 //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 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;
+    a = (a * (size / 2)) / 100;
-	b = (b * (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
     //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);
-	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
     //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
     //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
     //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
+    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, (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);
+    write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1);
 
     //pitch
     //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
-	write_line_outlined((x)-1, (y)-1, (x)-1 - k, (y)-1 - l,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, 5);
     //write_circle_outlined(x-1, y-1, 5,0,0,0,1);
@@ -1585,7 +1523,7 @@ void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t
 
 void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
 {
-	int i=0;
+    int i = 0;
     int batteryLines;
     //top
     /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
@@ -1600,30 +1538,30 @@ void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
      //right
      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 };
+    sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec);
     //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 updown=' ';
@@ -1636,7 +1574,7 @@ void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
  printTextFB(charx,y+2,temp);
  // frame
  drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
-}*/
+ }*/
 
 /**
  * hud_draw_vertical_scale: Draw a vertical scale.
@@ -1655,27 +1593,25 @@ 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, int max_val, int flags)
 {
-        char temp[15];//, temp2[15];
+    char temp[15];	//, temp2[15];
     struct FontEntry font_info;
     struct FontDimensions dim;
     // Halign should be in a small span.
     //MY_ASSERT(halign >= -1 && halign <= 1);
     // 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;
-        if(halign == -1)
+    if (halign == -1) {
-        {
         majtick_start = x;
         majtick_end = x + majtick_len;
         mintick_start = x;
         mintick_end = x + mintick_len;
         boundtick_start = x;
         boundtick_end = x + boundtick_len;
-        }
+    } else if (halign == +1) {
-        else if(halign == +1)
+        x = x - GRAPHICS_HDEADBAND;
-        {
-        		x=x-GRAPHICS_HDEADBAND;
         majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
         majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
         mintick_start = GRAPHICS_WIDTH_REAL - x - 1;
@@ -1693,69 +1629,61 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
     int range_2 = range / 2; //, height_2 = height / 2;
     int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
     // Iterate through each step.
-        for(r = -range_2; r <= +range_2; r++)
+    for (r = -range_2; r <= +range_2; r++) {
-        {
         style = 0;
         rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
         rv = -rr + range_2; // for number display
-                if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
+        if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
             rr += majtick_step / 2;
-                if(rr % majtick_step == 0)
+        if (rr % majtick_step == 0)
             style = 1; // major tick
-                else if(rr % mintick_step == 0)
+        else if (rr % mintick_step == 0)
             style = 2; // minor tick
         else
             style = 0;
-                if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
+        if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
             continue;
-                if(style)
+        if (style) {
-                {
             // Calculate y position.
-                        ys = ((long int)(r * height) / (long int)range) + y;
+            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)
+            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);
+                sprintf(temp, "%d", rv);
                 text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
-                                if(text_length > max_text_y)
+                if (text_length > max_text_y)
                     max_text_y = text_length;
-                                if(halign == -1)
+                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)
-                        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);
     //my_itoa(v, temp);
-        sprintf(temp,"%d",v);
+    sprintf(temp, "%d", v);
     // TODO: add auto-sizing.
     calc_text_dimensions(temp, font_info, 1, 0, &dim);
     int xx = 0, i = 0;
-        if(halign == -1)
+    if (halign == -1)
         xx = majtick_end + text_x_spacing;
     else
         xx = majtick_end - text_x_spacing;
     // Draw an arrow from the number to the point.
-        for(i = 0; i < arrow_len; i++)
+    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_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 {
-                else
-                {
             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);
@@ -1765,33 +1693,29 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
         // 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)
+    if (halign == -1) {
-        {
         write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 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);
-        }
+    } else {
-        else
-        {
         write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 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)
+    if (halign == -1)
         write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
     else
         write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
     // Then, add a slow cut off on the edges, so the text doesn't sharply
     // disappear. We simply clear the areas above and below the ticker, and we
     // use little markers on the edges.
-        if(halign == -1)
+    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,
-                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, 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);
-        else
+    } 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);
     }
@@ -1826,42 +1750,43 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
     textoffset = 8;
     int r, style, rr, xs; // rv,
     int range_2 = range / 2;
-        for(r = -range_2; r <= +range_2; r++)
+    for (r = -range_2; r <= +range_2; r++) {
-        {
         style = 0;
         rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
         //rv = -rr + range_2; // for number display
-                if(rr % majtick_step == 0)
+        if (rr % majtick_step == 0)
             style = 1; // major tick
-                else if(rr % mintick_step == 0)
+        else if (rr % mintick_step == 0)
             style = 2; // minor tick
-                if(style)
+        if (style) {
-                {
             // Calculate x position.
-                        xs = ((long int)(r * width) / (long int)range) + x;
+            xs = ((long int) (r * width) / (long int) range) + x;
             // Draw it.
-                        if(style == 1)
+            if (style == 1) {
-                        {
                 write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
                 // Draw heading above this tick.
                 // If it's not one of north, south, east, west, draw the heading.
                 // Otherwise, draw one of the identifiers.
-                                if(rr % 90 != 0)
+                if (rr % 90 != 0) {
-                                {
                     // We abbreviate heading to two digits. This has the side effect of being easy to compute.
                     headingstr[0] = '0' + (rr / 100);
                     headingstr[1] = '0' + ((rr / 10) % 10);
                     headingstr[2] = 0;
                     headingstr[3] = 0; // nul to terminate
-                                }
+                } else {
-                                else
+                    switch (rr) {
-                                {
+                        case 0:
-                                        switch(rr)
+                            headingstr[0] = 'N';
-                                        {
+                            break;
-                                                case 0:   headingstr[0] = 'N'; break;
+                        case 90:
-                                                case 90:  headingstr[0] = 'E'; break;
+                            headingstr[0] = 'E';
-                                                case 180: headingstr[0] = 'S'; break;
+                            break;
-                                                case 270: headingstr[0] = 'W'; break;
+                        case 180:
+                            headingstr[0] = 'S';
+                            break;
+                        case 270:
+                            headingstr[0] = 'W';
+                            break;
                     }
                     headingstr[1] = 0;
                     headingstr[2] = 0;
@@ -1869,8 +1794,7 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
                 }
                 // +1 fudge...!
                 write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
-                        }
+            } else if (style == 2)
-                        else if(style == 2)
                 write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
         }
     }
@@ -1886,226 +1810,201 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
     headingstr[1] = '0' + ((v / 10) % 10);
     headingstr[2] = '0' + (v % 10);
     headingstr[3] = 0;
-        write_string(headingstr, x + 1, majtick_start + textoffset+2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
+    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;
-	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
-	if((angle != 90) && (angle != -90))
+    if ((angle != 90) && (angle != -90)) {
-	{
         k = tanf(alpha);
         vertical = 0;
-		if(k==0)
+        if (k == 0) {
-		{
+            horizontal = 1;
-			horizontal=1;
         }
-	}
+    } else {
-	else
-	{
         vertical = 1;
     }
 
     // crossing point of line
-	if(!vertical && !horizontal)
+    if (!vertical && !horizontal) {
-	{
         // 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
-		x1=x;
+        x1 = x;
-		y1=y;
+        y1 = y;
-		if(y<0)
+        if (y < 0) {
-		{
+            y1 = 0;
-			y1=0;
+            x1 = ((y1 - y0) + k * x0) / k;
-			x1=((y1-y0)+k*x0)/k;
         }
-		if(y>size)
+        if (y > size) {
-		{
+            y1 = size;
-			y1=size;
+            x1 = ((y1 - y0) + k * x0) / k;
-			x1=((y1-y0)+k*x0)/k;
         }
         // left crossing point
-		x=size;
+        x = size;
-		y=k*(x-x0)+y0;
+        y = k * (x - x0) + y0;
-		x2=x;
+        x2 = x;
-		y2=y;
+        y2 = y;
-		if(y<0)
+        if (y < 0) {
-		{
+            y2 = 0;
-			y2=0;
+            x2 = ((y2 - y0) + k * x0) / k;
-			x2=((y2-y0)+k*x0)/k;
         }
-		if(y>size)
+        if (y > size) {
-		{
+            y2 = size;
-			y2=size;
+            x2 = ((y2 - y0) + k * x0) / k;
-			x2=((y2-y0)+k*x0)/k;
         }
         // move to location
         // horizon line
-		write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1);
+        write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1);
         //fill
-		if(angle<=0 && angle>-90)
+        if (angle <= 0 && angle > -90) {
-		{
             //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
-			for(int i=y2;i<size;i++)
+            for (int i = y2; i < size; i++) {
-			{
+                x2 = ((i - y0) + k * x0) / k;
-				x2=((i-y0)+k*x0)/k;
+                if (x2 > size) {
-				if(x2>size)
+                    x2 = size;
-					x2=size;
-				if(x2<0)
-					x2=0;
-				write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1);
                 }
+                if (x2 < 0) {
+                    x2 = 0;
                 }
-		else if(angle<-90)
+                write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1);
-		{
+            }
+        } else if (angle < -90) {
             //write_string("2", 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<y2;i++)
+            for (int i = 0; i < y2; i++) {
-			{
+                x2 = ((i - y0) + k * x0) / k;
-				x2=((i-y0)+k*x0)/k;
+                if (x2 > size) {
-				if(x2>size)
+                    x2 = size;
-					x2=size;
-				if(x2<0)
-					x2=0;
-				write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1);
                 }
+                if (x2 < 0) {
+                    x2 = 0;
                 }
-		else if(angle>0 && angle<90)
+                write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1);
-		{
+            }
+        } else if (angle > 0 && angle < 90) {
             //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++)
+            for (int i = y1; i < size; i++) {
-			{
+                x2 = ((i - y0) + k * x0) / k;
-				x2=((i-y0)+k*x0)/k;
+                if (x2 > size) {
-				if(x2>size)
+                    x2 = size;
-					x2=size;
-				if(x2<0)
-					x2=0;
-				write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1);
                 }
+                if (x2 < 0) {
+                    x2 = 0;
                 }
-		else if(angle>90)
+                write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1);
-		{
+            }
+        } else if (angle > 90) {
             //write_string("4", 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<y1;i++)
+            for (int i = 0; i < y1; i++) {
-			{
+                x2 = ((i - y0) + k * x0) / k;
-				x2=((i-y0)+k*x0)/k;
+                if (x2 > size) {
-				if(x2>size)
+                    x2 = size;
-					x2=size;
+                }
-				if(x2<0)
+                if (x2 < 0) {
-					x2=0;
+                    x2 = 0;
-				write_hline_lm(x2+l_x,0+l_x,i+l_y,1,1);
+                }
+                write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1);
             }
         }
-	}
+    } else if (vertical) {
-	else if(vertical)
-	{
         // horizon line
-		write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1);
+        write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1);
-		if(angle==90)
+        if (angle == 90) {
-		{
             //write_string("5", 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 = 0; i < size; i++) {
-			{
+                write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1);
-				write_hline_lm(0+l_x,x0+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);
-			for(int i=0;i<size;i++)
+            for (int i = 0; i < size; i++) {
-			{
+                write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1);
-				write_hline_lm(size+l_x,x0+l_x,i+l_y,1,1);
             }
         }
-	}
+    } else if (horizontal) {
-	else if(horizontal)
-	{
         // horizon line
-		write_hline_outlined(0+l_x,size+l_x,y0+l_y,0,0,0,1);
+        write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1);
-		if(angle<0)
+        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++)
+            for (int i = 0; i < y0; i++) {
-			{
+                write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
-				write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
             }
-		}
+        } else {
-		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++)
+            for (int i = y0; i < size; i++) {
-			{
+                write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
-				write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
             }
         }
     }
 
     //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
-	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;
+    int image = 0;
     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 */
-	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
     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;
-	HomeLocationGet (&home);
+    HomeLocationGet(&home);
     GPSPositionData gpsData;
-	GPSPositionGet (&gpsData);
+    GPSPositionGet(&gpsData);
 
     /** 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
     /**
@@ -2114,16 +2013,18 @@ void calcHomeArrow(int16_t m_yaw)
      Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
      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;
+    }
 
     // yaw corrected bearing, needs compass
-    u2g=brng-180-m_yaw;
+    u2g = brng - 180 - m_yaw;
-    if(u2g<0)
+    if (u2g < 0) {
-    	u2g+=360;
+        u2g += 360;
+    }
 
     // Haversine formula for distance
     /**
@@ -2136,60 +2037,58 @@ void calcHomeArrow(int16_t m_yaw)
      var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
      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 * atan2f(sqrtf(a), sqrtf(1 - a));
-            sinf((lon2-lon1)/2) * sinf((lon2-lon1)/2);
-    c = 2 * atan2f(sqrtf(a), sqrtf(1-a));
     d = 6371 * 1000 * c;
 
     // Elevation  v depends servo direction
-    if(d!=0)
+    if (d != 0)
-        elevation = 90-RAD2DEG(atanf(dAlt/d));
+        elevation = 90 - RAD2DEG(atanf(dAlt/d));
     else
         elevation = 0;
     //! TODO: sanity check
 
-	char temp[50]={0};
+    char temp[50] =
-	sprintf(temp,"hea:%d",(int)brng);
+    { 0 };
+    sprintf(temp, "hea:%d", (int) brng);
     write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
-	sprintf(temp,"ele:%d",(int)elevation);
+    sprintf(temp, "ele:%d", (int) elevation);
     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,"dis:%d",(int)d);
+    sprintf(temp, "dis:%d", (int) d);
     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,"u2g:%d",(int)u2g);
+    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);
 }
 
-int lama=10;
+int lama = 10;
 int lama_loc[2][30];
 
 void lamas(void)
 {
-	char temp[10]={0};
+    char temp[10] =
+    { 0 };
     lama++;
-	if(lama%10==0)
+    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);
+    OsdSettingsGet(&OsdSettings);
     AttitudeActualData attitude;
     AttitudeActualGet(&attitude);
     GPSPositionData gpsData;
@@ -2198,34 +2097,39 @@ void updateGraphics() {
     HomeLocationGet(&home);
     BaroAltitudeData baro;
     BaroAltitudeGet(&baro);
+    FlightStatusData status;
+    FlightStatusGet(&status);
+
+    PIOS_Servo_Set(0, OsdSettings.White);
+    PIOS_Servo_Set(1, OsdSettings.Black);
 
     switch (OsdSettings.Screen) {
         case 0: // Dave simple
         {
-			if(home.Set == HOMELOCATION_SET_FALSE)
+            if (home.Set == HOMELOCATION_SET_FALSE) {
-			{
+                char temps[20] =
-				char temps[20]={0};
+                { 0 };
-				sprintf(temps,"HOME NOT SET");
+                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);
+                write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
             }
 
-			
+            char temp[50] =
-			char temp[50]={0};
+            { 0 };
             memset(temp, ' ', 40);
-			sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
+            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);
+            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);
+            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));
+            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)
+            if (gpsData.Heading > 180)
-				calcHomeArrow((int16_t)(gpsData.Heading-360));
+                calcHomeArrow((int16_t)(gpsData.Heading - 360));
             else
                 calcHomeArrow((int16_t)(gpsData.Heading));
         }
@@ -2256,63 +2160,61 @@ void updateGraphics() {
              angleC+=2;*/
 
             // GPS HACK
-				if(gpsData.Heading>180)
+            if (gpsData.Heading > 180)
-					calcHomeArrow((int16_t)(gpsData.Heading-360));
+                calcHomeArrow((int16_t)(gpsData.Heading - 360));
             else
                 calcHomeArrow((int16_t)(gpsData.Heading));
 
             /* Draw Attitude Indicator */
-				if(OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED)
+            if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
-				{
+                drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),
-					drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]),attitude.Pitch,attitude.Roll,96);
+                        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};
+            char temp[50] =
+            { 0 };
             memset(temp, ' ', 40);
-				sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
+            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);
+            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);
+            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);
+            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)
+            if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
-				{
+                printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
-					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());
+            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);
+            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));
+            sprintf(temp, "Rssi:%4.2fV", (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);
+            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));
+            sprintf(temp, "Temp:%4.2fC", (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);
+            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));
+            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);
+            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));
+            sprintf(temp, "VidV:%4.2fV", (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);
+            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;
@@ -2335,27 +2237,21 @@ void updateGraphics() {
              }*/
 
             //drawAltitude(200,50,m_alt,dir);
-				
-				
             //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
-				
             // Draw airspeed (left side.)
-				if(OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED)
+            if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
-				{
+                hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
-					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)
+            if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
-				{
+                hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
-					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 (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
-				{
+                if (attitude.Yaw < 0) {
-					if(attitude.Yaw<0) {
+                    hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
-						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]),
@@ -2366,22 +2262,48 @@ void updateGraphics() {
             break;
         case 2:
         {
-			int size=64;
+            int size = 64;
-			int x=((GRAPHICS_RIGHT/2)-(size/2)),y=(GRAPHICS_BOTTOM-size-2);
+            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);
+            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)),
+            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,
-					APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
+                    10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
-			if(1)
+            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);
-				hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
+            } else {
-					APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
+                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);
-			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:
@@ -2389,25 +2311,36 @@ void updateGraphics() {
             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_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);
+            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_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 updateOnceEveryFrame() {
+void updateOnceEveryFrame()
+{
     clearGraphics();
     updateGraphics();
 }
 
-
 // ****************
 /**
  * Initialise the gps module
@@ -2418,10 +2351,12 @@ void updateOnceEveryFrame() {
 int32_t osdgenStart(void)
 {
     // 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);
-	//TaskMonitorAdd(TASKINFO_RUNNING_GPS, osdgenTaskHandle);
+    TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle);
-
+#ifdef PIOS_INCLUDE_WDG
+    PIOS_WDG_RegisterFlag(PIOS_WDG_OSDGEN);
+#endif
     return 0;
 }
 
@@ -2445,10 +2380,11 @@ int32_t osdgenInitialize(void)
 #endif
     OsdSettingsInitialize();
     BaroAltitudeInitialize();
+    FlightStatusInitialize();
 
     return 0;
 }
-MODULE_INITCALL(osdgenInitialize, osdgenStart)
+MODULE_INITCALL( osdgenInitialize, osdgenStart)
 
 // ****************
 /**
@@ -2460,30 +2396,38 @@ static void osdgenTask(void *parameters)
     //portTickType lastSysTime;
     // Loop forever
     //lastSysTime = xTaskGetTickCount();
+    OsdSettingsData OsdSettings;
+    OsdSettingsGet(&OsdSettings);
+
+    PIOS_Servo_Set(0, OsdSettings.White);
+    PIOS_Servo_Set(1, OsdSettings.Black);
 
     // intro
-	for(int i=0; i<63; i++)
+    for (int i = 0; i < 63; i++) {
-	{
+        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
-        if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
+#ifdef PIOS_INCLUDE_WDG
-        {
+            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
+#endif
             clearGraphics();
             introGraphics();
         }
     }
-	for(int i=0; i<63; i++)
+    for (int i = 0; i < 63; i++) {
-	{
+        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
-        if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
+#ifdef PIOS_INCLUDE_WDG
-        {
+            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
+#endif
             clearGraphics();
             introGraphics();
             introText();
         }
     }
 
-	while (1)
+    while (1) {
-	{
+        if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
-        if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
+#ifdef PIOS_INCLUDE_WDG
-        {
+            PIOS_WDG_UpdateFlag(PIOS_WDG_OSDGEN);
+#endif
             updateOnceEveryFrame();
         }
         //xSemaphoreTake(osdSemaphore, portMAX_DELAY);
@@ -2491,7 +2435,6 @@ static void osdgenTask(void *parameters)
     }
 }
 
-
 // ****************
 
 /**

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

@@ -29,63 +29,49 @@
  */
 
 // ****************
-
 #include "openpilot.h"
 #include "osdinput.h"
 #include "attitudeactual.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);
-	//TaskMonitorAdd(TASKINFO_RUNNING_GPS, OpOsdTaskHandle);
 
     return 0;
 }
@@ -94,9 +80,10 @@ int32_t OpOsdStart(void)
  * \return -1 if initialisation failed
  * \return 0 on success
  */
-int32_t OpOsdInitialize(void)
+int32_t osdinputInitialize(void)
 {
     AttitudeActualInitialize();
+    FlightStatusInitialize();
     // Initialize quaternion
     AttitudeActualData attitude;
     AttitudeActualGet(&attitude);
@@ -109,181 +96,80 @@ int32_t OpOsdInitialize(void)
     attitude.Yaw = 0;
     AttitudeActualSet(&attitude);
 
-
-	// TODO: Get gps settings object
     oposdPort = PIOS_COM_OSD;
 
-	oposd_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
+    oposd_rx_buffer = pvPortMalloc(MAX_PACKET_LENGTH);
     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(void *parameters)
 {
     portTickType xDelay = 100 / portTICK_RATE_MS;
     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;
-	//bool start_flag = false;
-	//bool found_cr = false;
-	//int32_t gpsRxOverflow = 0;
-
-	numUpdates = 0;
-	numChecksumErrors = 0;
-	numParsingErrors = 0;
-
-	timeOfLastUpdateMs = timeNowMs;
-	timeOfLastCommandMs = timeNowMs;
     uint8_t rx_count = 0;
     bool start_flag = false;
-	//bool found_cr = false;
+    int32_t osdRxOverflow = 0;
-	int32_t gpsRxOverflow = 0;
+    uint8_t c = 0xAA;
-	uint8_t c=0xAA;
     // Loop forever
-	while (1)
+    while (1) {
-	{
-		/*//DMA_Cmd(DMA1_Stream2, DISABLE);   //prohibit  channel3 for a little time
-		uint16_t cnt = DMA_GetCurrDataCounter(DMA1_Stream2);
-    	rx.wr = rx.buf_size-cnt;
-		if(rx.wr)
-		{
-			//PIOS_LED_Toggle(LED2);
-			while (	fifoBuf_getData(&rx, &c, 1) > 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 (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)
+        while (PIOS_COM_ReceiveBuffer(oposdPort, &c, 1, xDelay) > 0) {
-		{
 
             // detect start while acquiring stream
-			if (!start_flag && ((c == 0xCB) || (c == 0x34)))
+            if (!start_flag && ((c == 0xCB) || (c == 0x34))) {
-			{
                 start_flag = true;
                 rx_count = 0;
-			}
+            } else if (!start_flag) {
-			else
-			if (!start_flag)
                 continue;
+            }
 
-			if (rx_count >= 11)
+            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++;
+                osdRxOverflow++;
                 start_flag = false;
                 rx_count = 0;
-			}
+            } else {
-			else
-			{
                 oposd_rx_buffer[rx_count] = c;
                 rx_count++;
             }
-			if (rx_count == 11)
+            if (rx_count == 11) {
-			{
+                if (oposd_rx_buffer[1] == OSD_PKT_TYPE_ATT) {
-				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.Roll = (float) ((int16_t)(oposd_rx_buffer[3] | oposd_rx_buffer[4] << 8)) / 10.0f;
-					attitude.Pitch = (int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6]<<8);
+                    attitude.Pitch = (float) ((int16_t)(oposd_rx_buffer[5] | oposd_rx_buffer[6] << 8)) / 10.0f;
-					attitude.Yaw = (int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8]<<8);
+                    attitude.Yaw = (float) ((int16_t)(oposd_rx_buffer[7] | oposd_rx_buffer[8] << 8)) / 10.0f;
                     AttitudeActualSet(&attitude);
+                } else if (oposd_rx_buffer[1] == OSD_PKT_TYPE_MODE) {
+                    FlightStatusData status;
+                    FlightStatusGet(&status);
+                    status.Armed = oposd_rx_buffer[8];
+                    status.FlightMode = oposd_rx_buffer[3];
+                    FlightStatusSet(&status);
                 }
                 //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(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/PathPlanner/pathplanner.c

@@ -237,20 +237,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/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

@@ -53,6 +53,9 @@
 #include "relay_tuning.h"
 #include "virtualflybar.h"
 
+// Includes for various stabilization algorithms
+#include "relay_tuning.h"
+
 // Private constants
 #define MAX_QUEUE_SIZE 1
 
@@ -123,7 +126,6 @@ int32_t StabilizationInitialize()
 #ifdef DIAG_RATEDESIRED
 	RateDesiredInitialize();
 #endif
-
 	// Code required for relay tuning
 	sin_lookup_initalize();
 	RelayTuningSettingsInitialize();
@@ -211,7 +213,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];
@@ -277,6 +284,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)
@@ -292,6 +300,7 @@ static void stabilizationTask(void* parameters)
 
 					break;
 				}
+
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
 					if (reinit)
 						pids[PID_RATE_ROLL + i].iAccumulator = 0;
@@ -307,7 +316,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);

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)
@@ -115,7 +115,7 @@ int32_t SystemModStart(void)
     stackOverflow = false;
     mallocFailed = false;
     // 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
     TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
 
@@ -253,13 +253,12 @@ static void objectUpdatedCb(UAVObjEvent * ev)
         FlightStatusGet(&flightStatus);
 
         // 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;
         }
 
         // Execute action if disarmed
-		if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
+        if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
             retval = -1;
         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD) {
             if (objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT) {
@@ -270,11 +269,9 @@ static void objectUpdatedCb(UAVObjEvent * ev)
                 }
                 // Load selected instance
                 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();
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
                 retval = UAVObjLoadMetaobjects();
             }
         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE) {
@@ -293,11 +290,9 @@ static void objectUpdatedCb(UAVObjEvent * ev)
                 // Verify saving worked
                 if (retval == 0)
                     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();
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
                 retval = UAVObjSaveMetaobjects();
             }
         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE) {
@@ -309,11 +304,9 @@ static void objectUpdatedCb(UAVObjEvent * ev)
                 }
                 // Delete selected instance
                 retval = UAVObjDelete(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 = UAVObjDeleteSettings();
-			} else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS
+            } else if (objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
-				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
                 retval = UAVObjDeleteMetaobjects();
             }
         } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
@@ -323,7 +316,7 @@ static void objectUpdatedCb(UAVObjEvent * ev)
 			retval = -1;
 #endif
         }
-		switch(retval) {
+        switch (retval) {
             case 0:
                 objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
                 ObjectPersistenceSet(&objper);
@@ -384,7 +377,6 @@ static void updateWDGstats()
 }
 #endif
 
-
 /**
  * Called periodically to update the system stats
  */
@@ -393,10 +385,10 @@ static uint16_t GetFreeIrqStackSize(void)
     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;
@@ -456,17 +448,23 @@ static void updateStats()
     portTickType now = xTaskGetTickCount();
     if (now > lastTickCount) {
         uint32_t dT = (xTaskGetTickCount() - lastTickCount) * portTICK_RATE_MS;	// in ms
-		stats.CPULoad =
+        stats.CPULoad = 100 - (uint8_t) roundf(100.0f * ((float) idleCounter / ((float) dT / 1000.0f)) / (float) IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
-			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.43; /* V */
+    float temp_voltage = 3.3f * PIOS_ADC_PinGet(3) / ((1 << 12) - 1);
-	const float STM32_TEMP_AVG_SLOPE = 4.3; /* mV/C */
+    const float STM32_TEMP_V25 = 0.76f; /* V */
+    const float STM32_TEMP_AVG_SLOPE = 2.5f; /* mV/C */
     stats.CPUTemp = (temp_voltage-STM32_TEMP_V25) * 1000 / STM32_TEMP_AVG_SLOPE + 25;
+#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 / STM32_TEMP_AVG_SLOPE + 25;
+#endif
 #endif
     SystemStatsSet(&stats);
 }
@@ -482,15 +480,13 @@ static void updateSystemAlarms()
     SystemStatsGet(&stats);
 
     // 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)
 #endif
             ) {
         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)
 #endif

flight/modules/VtolPathFollower/vtolpathfollower.c

@@ -71,6 +71,11 @@
 #include "velocityactual.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
@@ -80,19 +85,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,7 +110,7 @@ int32_t VtolPathFollowerStart()
 {
     if (vtolpathfollower_enabled) {
         // 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);
         TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
     }
 
@@ -125,6 +133,10 @@ int32_t VtolPathFollowerInitialize()
         PathDesiredInitialize();
         PathStatusInitialize();
         VelocityDesiredInitialize();
+        CameraDesiredInitialize();
+        AccessoryDesiredInitialize();
+        PoiLearnSettingsInitialize();
+        PoiLocationInitialize();
         vtolpathfollower_enabled = true;
     } else {
         vtolpathfollower_enabled = false;
@@ -133,7 +145,7 @@ int32_t VtolPathFollowerInitialize()
     return 0;
 }
 
-MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart)
+MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart)
 
 static float northVelIntegral = 0;
 static float eastVelIntegral = 0;
@@ -151,15 +163,13 @@ static void vtolPathFollowerTask(void *parameters)
 {
     SystemSettingsData systemSettings;
     FlightStatusData flightStatus;
-	PathStatusData pathStatus;
 
     portTickType lastUpdateTime;
 
     VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
-	PathDesiredConnectCallback(SettingsUpdatedCb);
+    AccessoryDesiredConnectCallback(accessoryUpdated);
 
     VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
-	PathDesiredGet(&pathDesired);
 
     // Main task loop
     lastUpdateTime = xTaskGetTickCount();
@@ -171,19 +181,13 @@ static void vtolPathFollowerTask(void *parameters)
         //    FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
 
         SystemSettingsGet(&systemSettings);
-		if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
+        if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
+                && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) {
-			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
+            AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
-			(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;
         }
@@ -196,48 +200,57 @@ static void vtolPathFollowerTask(void *parameters)
 
         FlightStatusGet(&flightStatus);
         PathStatusGet(&pathStatus);
+        PathDesiredData pathDesired;
+        PathDesiredGet(&pathDesired);
 
         // Check the combinations of flightmode and pathdesired mode
-		switch(flightStatus.FlightMode) {
+        switch (flightStatus.FlightMode) {
+            case FLIGHTSTATUS_FLIGHTMODE_LAND:
             case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
             case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
                 if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
                     updateEndpointVelocity();
-					updateVtolDesiredAttitude();
+                    updateVtolDesiredAttitude(false);
-					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
                 } else {
-					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+                    AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
                 }
                 break;
             case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
                 pathStatus.UID = pathDesired.UID;
                 pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
-				switch(pathDesired.Mode) {
+                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();
+                        updateVtolDesiredAttitude(false);
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
                         break;
                     case PATHDESIRED_MODE_FIXEDATTITUDE:
                         updateFixedAttitude(pathDesired.ModeParameters);
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
                         break;
                     case PATHDESIRED_MODE_DISARMALARM:
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
+                        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
                         break;
                     default:
                         pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
-						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+                        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
@@ -261,6 +274,92 @@ static void vtolPathFollowerTask(void *parameters)
     }
 }
 
+/**
+ * Compute bearing and elevation between current position and POI
+ */
+static void updatePOIBearing()
+{
+    const float DEADBAND_HIGH = 0.10;
+    const float DEADBAND_LOW = -0.10;
+    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;
+    } else {
+        yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180;
+    }
+
+    // distance
+    float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2));
+
+    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 (pathAngle != 0 || changeRadius != 0) {
+            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;
+            pathDesired.EndingVelocity = 0;
+            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);
+    }
+}
+
 /**
  * Compute desired velocity from the current position and path
  *
@@ -272,46 +371,69 @@ static void updatePathVelocity()
     float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
     float downCommand;
 
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
     PositionActualData positionActual;
     PositionActualGet(&positionActual);
 
-	float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
+    float cur[3] =
+    { positionActual.North, positionActual.East, positionActual.Down };
     struct path_status progress;
 
     path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
 
-	float groundspeed = pathDesired.StartingVelocity + 
+    float groundspeed;
-	    (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1);
+    switch (pathDesired.Mode) {
-	if(progress.fractional_progress > 1)
+        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, 
+    float correction_velocity[2] =
-	    progress.correction_direction[1] * error_speed};
+    { 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 total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
     float scale = 1;
-	if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
+    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]) *
+    float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1);
-	    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,
+    velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
-								 -vtolpathfollowerSettings.VerticalVelMax,
+
-								 vtolpathfollowerSettings.VerticalVelMax);
+    // update pathstatus
+    pathStatus.error = progress.error;
+    pathStatus.fractional_progress = progress.fractional_progress;
 
     VelocityDesiredSet(&velocityDesired);
 }
@@ -326,6 +448,9 @@ void updateEndpointVelocity()
 {
     float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
 
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
     PositionActualData positionActual;
     VelocityDesiredData velocityDesired;
 
@@ -378,20 +503,18 @@ void updateEndpointVelocity()
     northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
             -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] +
+    eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
-		       eastPosIntegral);
 
     // Limit the maximum velocity
-	float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
+    float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
     float scale = 1;
-	if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
+    if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
         scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
 
     velocityDesired.North = northCommand * scale;
@@ -402,9 +525,7 @@ void updateEndpointVelocity()
             -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
             vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
     downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
-	velocityDesired.Down = bound(downCommand,
+    velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
-				     -vtolpathfollowerSettings.VerticalVelMax, 
-				     vtolpathfollowerSettings.VerticalVelMax);
 
     VelocityDesiredSet(&velocityDesired);
 }
@@ -434,7 +555,7 @@ 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;
 
@@ -500,27 +621,24 @@ static void updateVtolDesiredAttitude()
     // Testing code - refactor into manual control command
     ManualControlCommandData manualControlData;
     ManualControlCommandGet(&manualControlData);
-	stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;	
 
     // Compute desired north command
     northError = velocityDesired.North - northVel;
     northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
             -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
             vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
-	northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
+    northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
-			northVelIntegral -
+            - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
-			nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
+            + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
-			velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
 
     // Compute desired east command
     eastError = velocityDesired.East - eastVel;
     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] + 
+    eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
-		       eastVelIntegral - 
+            - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
-		       nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
+            + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
-			   velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
 
     // Compute desired down command
     downError = velocityDesired.Down - downVel;
@@ -529,9 +647,8 @@ static void updateVtolDesiredAttitude()
     downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
             -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
             vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
-	downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] +
+    downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
-		       downVelIntegral -
+            - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
-		       nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
 
     stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
 
@@ -544,7 +661,7 @@ static void updateVtolDesiredAttitude()
 				     eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)),
             -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.
         ManualControlCommandData manualControl;
         ManualControlCommandGet(&manualControl);
@@ -553,8 +670,12 @@ static void updateVtolDesiredAttitude()
 
     stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
     stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    if (yaw_attitude) {
+        stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    } else {
         stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-	
+        stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
+    }
     StabilizationDesiredSet(&stabDesired);
 }
 
@@ -578,15 +699,15 @@ static void updateNedAccel()
     //rotate avg accels into earth frame and store it
     AttitudeActualData 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);
-	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;
 
@@ -614,6 +735,30 @@ static float bound(float val, float min, float max)
 static void SettingsUpdatedCb(UAVObjEvent * ev)
 {
     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/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;
 
+	struct pios_hcsr04_dev * hcsr04_dev;
+
+	hcsr04_dev = (struct pios_hcsr04_dev *) PIOS_PWM_alloc();
+	if (!hcsr04_dev) goto out_fail;
+
+	/* Bind the configuration to the device instance */
+	hcsr04_dev->cfg = cfg;
+	hcsr04_dev_loc = hcsr04_dev;
+
+	for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
 		/* Flush counter variables */
-	CaptureState = 0;
+		hcsr04_dev->CaptureState[i] = 0;
-	RiseValue = 0;
+		hcsr04_dev->RiseValue[i] = 0;
-	FallValue = 0;
+		hcsr04_dev->FallValue[i] = 0;
-	CaptureValue = 0;
+		hcsr04_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
+	}
 
-	/* Setup RCC */
+	uint32_t tim_id;
-	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+	if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)hcsr04_dev)) {
+		return -1;
+	}
 
-	/* Enable timer interrupts */
+	/* Configure the channels to be in capture/compare mode */
-	NVIC_InitTypeDef NVIC_InitStructure;
+	for (uint8_t i = 0; i < cfg->num_channels; i++) {
-	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID;
+		const struct pios_tim_channel * chan = &cfg->channels[i];
-	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) */
-	GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);
-
-	/* Configure input pins */
-	GPIO_StructInit(&GPIO_InitStructure);
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
-	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
-	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
-	GPIO_Init(GPIOB, &GPIO_InitStructure);
 
 		/* Configure timer for input capture */
-	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+		TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
-	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
-	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
-	TIM_ICInitStructure.TIM_ICFilter = 0x0;
-	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
-	TIM_ICInit(TIM3, &TIM_ICInitStructure);
-
-	/* Configure timer clocks */
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
-	TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
-	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 */
-	//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);
+
+	if (channel >= hcsr04_dev->cfg->num_channels) {
+		/* 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 {
-			FallValue = TIM_GetCapture2(TIM3);
+		hcsr04_dev->FallValue[chan_idx] = count;
-		}
 	}
 
-	/* Clear TIM3 Capture compare interrupt pending bit */
+	// flip state machine and capture value here
-	TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
-
 	/* 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_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.
@@ -50,7 +57,7 @@ struct _buffers
     uint8_t buffer0_mask[GRAPHICS_HEIGHT*GRAPHICS_WIDTH];
     uint8_t buffer1_level[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;
 
 /**
@@ -86,59 +94,43 @@ void swap_buffers()
     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);
-			DMA_Cmd(dev_cfg->mask.dma.tx.channel, ENABLE);
     }
-	} else {
+    Hsync_update++;
-		//falling
+    return true;
-		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;
-	//PIOS_LED_Toggle(LED3);
 
-	//if(gActiveLine > 200)
     xHigherPriorityTaskWoken = pdFALSE;
     m_osdLines = gActiveLine;
-	{
+
+    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;
@@ -148,16 +140,169 @@ uint16_t PIOS_Video_GetOSDLines(void) {
     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);
+}
 
+const struct pios_tim_callbacks px_callback = {
+    .overflow = NULL,
+    .edge = NULL,
+};
+
+#ifdef PAL
+const uint32_t period = 10;
+const uint32_t dc = (10 / 2);
+#else
+const uint32_t period = 11;
+const uint32_t dc = (11 / 2);
+#endif
+/**
+ * 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
+ */
+uint32_t failcount = 0;
+static void reset_hsync_timers()
+{
+    // Stop both timers
+    TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
+
+    uint32_t tim_id;
+    const struct pios_tim_channel *channels = &dev_cfg->hsync_capture;
+
+    //BUG: This is nuts this line is needed.  It simply results in allocating
+    //all the memory but somehow leaving it out breaks the timer functionality.
+    // 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++;
+    }
+
+    dev_cfg->pixel_timer.timer->CNT = 0xFFFF - 100; //dc;
+
+    // Listen to Channel1 (HSYNC)
+    switch(dev_cfg->hsync_capture.timer_chan) {
+        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);
+}
+
+static void configure_hsync_timers()
+{
+    // Stop both timers
+    TIM_Cmd(dev_cfg->pixel_timer.timer, DISABLE);
+
+    // This is overkill but used for consistency.  No interrupts used for pixel clock
+    // but this function calls the GPIO_Remap
+    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);
+
+    // Init the channel to capture the pulse
+    channels = &dev_cfg->hsync_capture;
+    PIOS_TIM_InitChannels(&tim_id, channels, 1, &px_callback, 0);
+
+    // Configure the input capture channel
+    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);
+
+    // Set up the channel to output the pixel clock
+    switch(dev_cfg->pixel_timer.timer_chan) {
+        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);
+
+    // This shouldn't be needed as it should come from the config struture.  Something
+    // is clobbering that
+    TIM_PrescalerConfig(dev_cfg->pixel_timer.timer, 0, TIM_PSCReloadMode_Immediate);
+    TIM_SetAutoreload(dev_cfg->pixel_timer.timer, period);
+}
+
+DMA_TypeDef * main_dma;
+DMA_TypeDef * mask_dma;
+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.mosi.gpio,
+        GPIO_PinAFConfig(cfg->mask.miso.gpio,
-				__builtin_ctz(cfg->mask.mosi.init.GPIO_Pin),
+                __builtin_ctz(cfg->mask.miso.init.GPIO_Pin),
                 cfg->mask.remap);
     }
     if (cfg->level.remap)
@@ -170,14 +315,6 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg){
                 cfg->level.remap);
     }
 
-	/* SPI3 MASTER MASKBUFFER */
-	GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init));
-	GPIO_Init(cfg->mask.mosi.gpio, (GPIO_InitTypeDef*)&(cfg->mask.mosi.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));
-
     /* Initialize the SPI block */
     SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init));
     SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init));
@@ -186,96 +323,158 @@ void PIOS_Video_Init(const struct pios_video_cfg * cfg){
     SPI_Cmd(cfg->level.regs, ENABLE);
     SPI_Cmd(cfg->mask.regs, ENABLE);
 
-	/* Configure DMA for SPI Tx MASTER */
+    /* 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 */
+    /* 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->mask.dma.tx.channel, DMA_IT_TC, ENABLE);
+    DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE);
-	/*DMA_ClearFlag(cfg->mask.dma.tx.channel,DMA_FLAG_TCIF5);
-	DMA_ClearITPendingBit(cfg->mask.dma.tx.channel, DMA_IT_TCIF5);
-
-	DMA_ClearFlag(cfg->level.dma.tx.channel,DMA_FLAG_TCIF5);
-	DMA_ClearITPendingBit(cfg->level.dma.tx.channel, DMA_IT_TCIF5);
-*/
-
-	/* Configure DMA interrupt */
-	NVIC_Init(&cfg->level.dma.irq.init);
-	NVIC_Init(&cfg->mask.dma.irq.init);
-
-	/* Enable SPI interrupts to DMA */
-	SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
-	SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
-
-	/* Configure the Video Line interrupt */
-	PIOS_EXTI_Init(cfg->hsync);
-	PIOS_EXTI_Init(cfg->vsync);
-
 
+    /* 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();
+            stop_hsync_timers();
+
+            dev_cfg->pixel_timer.timer->CNT = dc;
+
+            prepare_line(gActiveLine);
+        }
+        else if(gActiveLine >= GRAPHICS_HEIGHT)
+        {
+            //last line completed
+            flush_spi();
+            stop_hsync_timers();
+
+            // STOP DMA, master first
             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_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)) {
+        gActiveLine++;
-		DMA_ClearFlag(DMA1_Stream7,DMA_FLAG_HTIF7);
+    }
+    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 {
-
     }
-
-	if (DMA_GetFlagStatus(DMA2_Stream5,DMA_FLAG_TCIF5)) {	// whole double buffer filled
-		DMA_ClearFlag(DMA2_Stream5,DMA_FLAG_TCIF5);
-		//PIOS_LED_Toggle(LED3);
-	}
-	else if (DMA_GetFlagStatus(DMA2_Stream5,DMA_FLAG_HTIF5)) {
-		DMA_ClearFlag(DMA2_Stream5,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_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/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/stm32f4xx/libraries/CMSIS2/Device/ST/STM32F4xx/Source/osd/system_stm32f4xx.c

@@ -2,12 +2,12 @@
  ******************************************************************************
  * @file    system_stm32f4xx.c
  * @author  MCD Application Team
-  * @version V1.0.1
+ * @version V1.0.0
-  * @date    24-January-2012
+ * @date    30-September-2011
  * @brief   CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
  *          This file contains the system clock configuration for STM32F4xx devices,
  *          and is generated by the clock configuration tool
-  *          stm32f4xx_Clock_Configuration_V1.0.1.xls
+ *          stm32f4xx_Clock_Configuration_V1.0.0.xls
  *             
  * 1.  This file provides two functions and one global variable to be called from 
  *     user application:
@@ -46,9 +46,9 @@
  *-----------------------------------------------------------------------------
  *        System Clock source                    | PLL (HSE)
  *-----------------------------------------------------------------------------
-  *        SYSCLK(Hz)                             | 108000000
+ *        SYSCLK(Hz)                             | 168000000
  *-----------------------------------------------------------------------------
-  *        HCLK(Hz)                               | 108000000
+ *        HCLK(Hz)                               | 168000000
  *-----------------------------------------------------------------------------
  *        AHB Prescaler                          | 1
  *-----------------------------------------------------------------------------
@@ -58,13 +58,13 @@
  *-----------------------------------------------------------------------------
  *        HSE Frequency(Hz)                      | 8000000
  *-----------------------------------------------------------------------------
-  *        PLL_M                                  | 4
+ *        PLL_M                                  | 10
  *-----------------------------------------------------------------------------
-  *        PLL_N                                  | 216
+ *        PLL_N                                  | 420
  *-----------------------------------------------------------------------------
-  *        PLL_P                                  | 4
+ *        PLL_P                                  | 2
  *-----------------------------------------------------------------------------
-  *        PLL_Q                                  | 9
+ *        PLL_Q                                  | 7
  *-----------------------------------------------------------------------------
  *        PLLI2S_N                               | NA
  *-----------------------------------------------------------------------------
@@ -74,9 +74,9 @@
  *-----------------------------------------------------------------------------
  *        VDD(V)                                 | 3.3
  *-----------------------------------------------------------------------------
-  *        Main regulator output voltage          | Scale2 mode
+ *        Main regulator output voltage          | Scale1 mode
  *-----------------------------------------------------------------------------
-  *        Flash Latency(WS)                      | 3
+ *        Flash Latency(WS)                      | 5
  *-----------------------------------------------------------------------------
  *        Prefetch Buffer                        | OFF
  *-----------------------------------------------------------------------------
@@ -84,7 +84,7 @@
  *-----------------------------------------------------------------------------
  *        Data cache                             | ON
  *-----------------------------------------------------------------------------
-  *        Require 48MHz for USB OTG FS,          | Disabled
+ *        Require 48MHz for USB OTG FS,          | Enabled
  *        SDIO and RNG clock                     |
  *-----------------------------------------------------------------------------
  *=============================================================================
@@ -141,19 +141,19 @@
  Internal SRAM. */
 /* #define VECT_TAB_SRAM */
 #define VECT_TAB_OFFSET  0x00 /*!< Vector Table base offset field. 
-                                   This value must be a multiple of 0x200. */
+This value must be a multiple of 0x200. */
 /******************************************************************************/
 
 /************************* PLL Parameters *************************************/
 /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */
-#define PLL_M      4
+#define PLL_M      10
-#define PLL_N      216
+#define PLL_N      420
 
 /* SYSCLK = PLL_VCO / PLL_P */
-#define PLL_P      4
+#define PLL_P      2
 
 /* USB OTG FS, SDIO and RNG Clock =  PLL_VCO / PLLQ */
-#define PLL_Q      9
+#define PLL_Q      7
 
 /******************************************************************************/
 
@@ -173,9 +173,9 @@
  * @{
  */
 
-  uint32_t SystemCoreClock = 108000000;
+uint32_t SystemCoreClock = 168000000;
 
-  __I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
 
 /**
  * @}
@@ -187,7 +187,7 @@
 
 static void SetSysClock(void);
 #ifdef DATA_IN_ExtSRAM
-  static void SystemInit_ExtMemCtl(void); 
+static void SystemInit_ExtMemCtl(void); 
 #endif /* DATA_IN_ExtSRAM */
 
 /**
@@ -208,9 +208,10 @@ static void SetSysClock(void);
 void SystemInit(void)
 {
 	/* FPU settings ------------------------------------------------------------*/
-  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
     SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2));  /* set CP10 and CP11 Full Access */
-  #endif
+#endif
+	
 	/* Reset the RCC clock configuration to the default reset state ------------*/
 	/* Set HSION bit */
 	RCC->CR |= (uint32_t)0x00000001;
@@ -340,9 +341,9 @@ void SystemCoreClockUpdate(void)
  */
 static void SetSysClock(void)
 {
-/******************************************************************************/
+	/******************************************************************************/
-/*            PLL (clocked by HSE) used as System clock source                */
+	/*            PLL (clocked by HSE) used as System clock source                */
-/******************************************************************************/
+	/******************************************************************************/
 	__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
 	
 	/* Enable HSE */
@@ -366,9 +367,9 @@ static void SetSysClock(void)
 	
 	if (HSEStatus == (uint32_t)0x01)
 	{
-    /* Select regulator voltage output Scale 2 mode, System frequency up to 144 MHz */
+		/* Select regulator voltage output Scale 1 mode, System frequency up to 168 MHz */
 		RCC->APB1ENR |= RCC_APB1ENR_PWREN;
-    PWR->CR &= (uint32_t)~(PWR_CR_VOS);
+		PWR->CR |= PWR_CR_VOS;
 		
 		/* HCLK = SYSCLK / 1*/
 		RCC->CFGR |= RCC_CFGR_HPRE_DIV1;
@@ -392,7 +393,7 @@ static void SetSysClock(void)
 		}
 		
 		/* Configure Flash prefetch, Instruction cache, Data cache and wait state */
-    FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_3WS;
+		FLASH->ACR = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;
 		
 		/* Select the main PLL as system clock source */
 		RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
@@ -427,8 +428,8 @@ static void SetSysClock(void)
  */
 void SystemInit_ExtMemCtl(void)
 {
-/*-- GPIOs Configuration -----------------------------------------------------*/
+	/*-- GPIOs Configuration -----------------------------------------------------*/
-/*
+	/*
 	 +-------------------+--------------------+------------------+------------------+
 	 +                       SRAM pins assignment                                   +
 	 +-------------------+--------------------+------------------+------------------+
@@ -446,7 +447,7 @@ void SystemInit_ExtMemCtl(void)
 	 | PD15 <-> FSMC_D1  | PE14 <-> FSMC_D11  |
 	 |                   | PE15 <-> FSMC_D12  |
 	 +-------------------+--------------------+
-*/
+	 */
 	/* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
 	RCC->AHB1ENR   = 0x00000078;
 	
@@ -498,7 +499,7 @@ void SystemInit_ExtMemCtl(void)
 	/* No pull-up, pull-down for PGx pins */ 
 	GPIOG->PUPDR   = 0x00000000;
 	
-/*-- FSMC Configuration ------------------------------------------------------*/
+	/*-- FSMC Configuration ------------------------------------------------------*/
 	/* Enable the FSMC interface clock */
 	RCC->AHB3ENR         = 0x00000001;
 	
@@ -532,7 +533,8 @@ void SystemInit_ExtMemCtl(void)
 	 FSMC_NORSRAMInitStructure.FSMC_WriteBurst = FSMC_WriteBurst_Disable;
 	 FSMC_NORSRAMInitStructure.FSMC_ReadWriteTimingStruct = &p;
 	 FSMC_NORSRAMInitStructure.FSMC_WriteTimingStruct = &p;
-*/  
+	 */
+	
 }
 #endif /* DATA_IN_ExtSRAM */
 
@@ -549,4 +551,3 @@ void SystemInit_ExtMemCtl(void)
  * @}
  */    
 /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
-

flight/pios/stm32f4xx/pios_sys.c

@@ -43,8 +43,8 @@ void SysTick_Handler(void);
 #define MEM32(addr)  (*((volatile uint32_t  *)(addr)))
 
 /**
-* Initialises all system peripherals
+ * Initialises all system peripherals
-*/
+ */
 void PIOS_SYS_Init(void)
 {
     /* Setup STM32 system (RCC, clock, PLL and Flash configuration) - CMSIS Function */
@@ -162,7 +162,7 @@ void PIOS_SYS_Init(void)
     GPIO_Init(GPIOA, &GPIO_InitStructure);
 
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
-	GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4);					// leave JTAG pins alone
+    GPIO_InitStructure.GPIO_Pin &= ~(GPIO_Pin_3 | GPIO_Pin_4);// leave JTAG pins alone
     GPIO_Init(GPIOB, &GPIO_InitStructure);
 
     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
@@ -176,15 +176,15 @@ void PIOS_SYS_Init(void)
 }
 
 /**
-* Shutdown PIOS and reset the microcontroller:<BR>
+ * Shutdown PIOS and reset the microcontroller:<BR>
-* <UL>
+ * <UL>
-*   <LI>Disable all RTOS tasks
+ *   <LI>Disable all RTOS tasks
-*   <LI>Disable all interrupts
+ *   <LI>Disable all interrupts
-*   <LI>Turn off all board LEDs
+ *   <LI>Turn off all board LEDs
-*   <LI>Reset STM32
+ *   <LI>Reset STM32
-* </UL>
+ * </UL>
-* \return < 0 if reset failed
+ * \return < 0 if reset failed
-*/
+ */
 int32_t PIOS_SYS_Reset(void)
 {
     /* Disable all RTOS tasks */
@@ -209,15 +209,15 @@ int32_t PIOS_SYS_Reset(void)
     /* Reset STM32 */
     NVIC_SystemReset();
 
-	while (1) ;
+    while (1);
 
     /* We will never reach this point */
     return -1;
 }
 
 /**
-* Returns the CPU's flash size (in bytes)
+ * Returns the CPU's flash size (in bytes)
-*/
+ */
 uint32_t PIOS_SYS_getCPUFlashSize(void)
 {
     return ((uint32_t) MEM16(0x1fff7a22) * 1024);	// it might be possible to locate in the OTP area, but haven't looked and not documented
@@ -245,11 +245,11 @@ int32_t PIOS_SYS_SerialNumberGetBinary(uint8_t *array)
 }
 
 /**
-* Returns the serial number as a string
+ * Returns the serial number as a string
-* param[out] str pointer to a string which can store at least 32 digits + zero terminator!
+ * param[out] str pointer to a string which can store at least 32 digits + zero terminator!
-* (24 digits returned for STM32)
+ * (24 digits returned for STM32)
-* return < 0 if feature not supported
+ * return < 0 if feature not supported
-*/
+ */
 int32_t PIOS_SYS_SerialNumberGet(char *str)
 {
     int i;
@@ -270,8 +270,8 @@ int32_t PIOS_SYS_SerialNumberGet(char *str)
 }
 
 /**
-* Configures Vector Table base location and SysTick
+ * Configures Vector Table base location and SysTick
-*/
+ */
 void NVIC_Configuration(void)
 {
     /* Set the Vector Table base address as specified in .ld file */
@@ -287,12 +287,12 @@ void NVIC_Configuration(void)
 
 #ifdef USE_FULL_ASSERT
 /**
-* Reports the name of the source file and the source line number
+ * Reports the name of the source file and the source line number
-*   where the assert_param error has occurred.
+ *   where the assert_param error has occurred.
-* \param[in]  file pointer to the source file name
+ * \param[in]  file pointer to the source file name
-* \param[in]  line assert_param error line source number
+ * \param[in]  line assert_param error line source number
-* \retval None
+ * \retval None
-*/
+ */
 void assert_failed(uint8_t * file, uint32_t line)
 {
     /* When serial debugging is implemented, use something like this. */
@@ -314,7 +314,7 @@ void assert_failed(uint8_t * file, uint32_t line)
 #if defined(PIOS_LED_ALARM)
         PIOS_LED_Toggle(PIOS_LED_ALARM);
 #endif	/* PIOS_LED_ALARM */
-		for (int i = 0; i < 1000000; i++) ;
+        for (int i = 0; i < 1000000; i++);
     }
 }
 #endif

flight/targets/boards/coptercontrol/board_hw_defs.c

@@ -1067,6 +1067,82 @@ static const struct pios_sbus_cfg pios_sbus_cfg = {
 
 #endif	/* PIOS_INCLUDE_SBUS */
 
+/*
+ * HK OSD
+ */
+static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
+  .regs = USART1,
+  .init = {
+    .USART_BaudRate            = 57600,
+    .USART_WordLength          = USART_WordLength_8b,
+    .USART_Parity              = USART_Parity_No,
+    .USART_StopBits            = USART_StopBits_1,
+    .USART_HardwareFlowControl = USART_HardwareFlowControl_None,
+    .USART_Mode                = USART_Mode_Rx | USART_Mode_Tx,
+  },
+  .irq = {
+    .init    = {
+      .NVIC_IRQChannel                   = USART1_IRQn,
+      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+      .NVIC_IRQChannelSubPriority        = 0,
+      .NVIC_IRQChannelCmd                = ENABLE,
+    },
+  },
+  .rx   = {
+    .gpio = GPIOA,
+    .init = {
+      .GPIO_Pin   = GPIO_Pin_10,
+      .GPIO_Speed = GPIO_Speed_2MHz,
+      .GPIO_Mode  = GPIO_Mode_IPU,
+    },
+  },
+  .tx   = {
+    .gpio = GPIOA,
+    .init = {
+      .GPIO_Pin   = GPIO_Pin_9,
+      .GPIO_Speed = GPIO_Speed_2MHz,
+      .GPIO_Mode  = GPIO_Mode_AF_PP,
+    },
+  },
+};
+
+static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
+  .regs = USART3,
+  .init = {
+    .USART_BaudRate            = 57600,
+    .USART_WordLength          = USART_WordLength_8b,
+    .USART_Parity              = USART_Parity_No,
+    .USART_StopBits            = USART_StopBits_1,
+    .USART_HardwareFlowControl = USART_HardwareFlowControl_None,
+    .USART_Mode                = USART_Mode_Rx | USART_Mode_Tx,
+  },
+  .irq = {
+    .init    = {
+      .NVIC_IRQChannel                   = USART3_IRQn,
+      .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+      .NVIC_IRQChannelSubPriority        = 0,
+      .NVIC_IRQChannelCmd                = ENABLE,
+    },
+  },
+  .rx   = {
+    .gpio = GPIOB,
+    .init = {
+      .GPIO_Pin   = GPIO_Pin_11,
+      .GPIO_Speed = GPIO_Speed_2MHz,
+      .GPIO_Mode  = GPIO_Mode_IPU,
+    },
+  },
+  .tx   = {
+    .gpio = GPIOB,
+    .init = {
+      .GPIO_Pin   = GPIO_Pin_10,
+      .GPIO_Speed = GPIO_Speed_2MHz,
+      .GPIO_Mode  = GPIO_Mode_AF_PP,
+    },
+  },
+};
+
+
 #endif  /* PIOS_INCLUDE_USART */
 
 #if defined(PIOS_INCLUDE_COM)
@@ -1208,6 +1284,50 @@ const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = {
 
 #endif
 
+
+
+/*
+ * SONAR Inputs
+ */
+#if defined(PIOS_INCLUDE_HCSR04)
+#include <pios_hcsr04_priv.h>
+
+static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = {
+{
+	.timer = TIM3,
+	.timer_chan = TIM_Channel_2,
+	.pin = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_5,
+			.GPIO_Mode  = GPIO_Mode_IPD,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+		},
+	},
+	.remap = GPIO_PartialRemap_TIM3,
+},
+};
+
+const struct pios_hcsr04_cfg pios_hcsr04_cfg = {
+	.tim_ic_init = {
+		.TIM_ICPolarity = TIM_ICPolarity_Rising,
+		.TIM_ICSelection = TIM_ICSelection_DirectTI,
+		.TIM_ICPrescaler = TIM_ICPSC_DIV1,
+		.TIM_ICFilter = 0x0,
+	},
+	.channels = pios_tim_hcsr04_port_all_channels,
+	.num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels),
+	.trigger = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_6,
+			.GPIO_Mode  = GPIO_Mode_Out_PP,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+		},
+	},
+};
+#endif
+
 #if defined(PIOS_INCLUDE_I2C)
 
 #include <pios_i2c_priv.h>

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -43,6 +43,7 @@ OPTMODULES += CameraStab
 OPTMODULES += ComUsbBridge
 OPTMODULES += GPS
 OPTMODULES += TxPID
+OPTMODULES += Osd/osdoutput
 #OPTMODULES += Altitude
 #OPTMODULES += Fault
 

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

@@ -59,6 +59,8 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 #define PIOS_COM_BRIDGE_RX_BUF_LEN 65
 #define PIOS_COM_BRIDGE_TX_BUF_LEN 12
 
+#define PIOS_COM_HKOSD_TX_BUF_LEN 22
+
 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
 #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
 uint32_t pios_com_debug_id;
@@ -69,6 +71,7 @@ uint32_t pios_com_telem_usb_id;
 uint32_t pios_com_vcp_id;
 uint32_t pios_com_gps_id;
 uint32_t pios_com_bridge_id;
+uint32_t pios_com_hkosd_id;
 
 uint32_t pios_usb_rctx_id;
 
@@ -546,6 +549,22 @@ void PIOS_Board_Init(void) {
 			}
 		}
 		break;
+	case HWSETTINGS_CC_MAINPORT_OSDHK:
+		{
+			uint32_t pios_usart_hkosd_id;
+			if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) {
+				PIOS_Assert(0);
+			}
+
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN);
+			PIOS_Assert(tx_buffer);
+			if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id,
+						NULL, 0,
+						tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) {
+				PIOS_Assert(0);
+			}
+		}
+		break;
 	}
 
 	/* Configure the flexi port */
@@ -695,6 +714,22 @@ void PIOS_Board_Init(void) {
 		}
 #endif	/* PIOS_INCLUDE_I2C */
 		break;
+	case HWSETTINGS_CC_FLEXIPORT_OSDHK:
+		{
+			uint32_t pios_usart_hkosd_id;
+			if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) {
+				PIOS_Assert(0);
+			}
+
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_HKOSD_TX_BUF_LEN);
+			PIOS_Assert(tx_buffer);
+			if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id,
+						NULL, 0,
+						tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) {
+				PIOS_Assert(0);
+			}
+		}
+		break;
 	}
 
 	/* Configure the rcvr port */
@@ -703,6 +738,12 @@ void PIOS_Board_Init(void) {
 
 	switch (hwsettings_rcvrport) {
 	case HWSETTINGS_CC_RCVRPORT_DISABLED:
+#if defined(PIOS_INCLUDE_HCSR04)
+		{
+			uint32_t pios_hcsr04_id;
+			PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg);
+		}
+#endif
 		break;
 	case HWSETTINGS_CC_RCVRPORT_PWM:
 #if defined(PIOS_INCLUDE_PWM)

flight/targets/boards/coptercontrol/pios_board.h

@@ -1,4 +1,4 @@
- /**
+/**
  ******************************************************************************
  *
  * @file       pios_board.h
@@ -30,14 +30,14 @@
 // Timers and Channels Used
 //------------------------
 /*
-Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
+ Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
-------+-----------+-----------+-----------+----------
+ ------+-----------+-----------+-----------+----------
-TIM1  |  Servo 4  |           |           |
+ TIM1  |  Servo 4  |           |           |
-TIM2  |  RC In 5  |  RC In 6  |  Servo 6  |
+ TIM2  |  RC In 5  |  RC In 6  |  Servo 6  |
-TIM3  |  Servo 5  |  RC In 2  |  RC In 3  |  RC In 4
+ TIM3  |  Servo 5  |  RC In 2  |  RC In 3  |  RC In 4
-TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
+ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
-------+-----------+-----------+-----------+----------
+ ------+-----------+-----------+-----------+----------
-*/
+ */
 
 //------------------------
 // DMA Channels Used
@@ -55,7 +55,6 @@ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
 /* Channel 11 - 				*/
 /* Channel 12 - 				*/
 
-
 //------------------------
 // BOOTLOADER_SETTINGS
 //------------------------
@@ -63,7 +62,6 @@ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
 #define BOARD_WRITABLE	TRUE
 #define MAX_DEL_RETRYS	3
 
-
 //------------------------
 // WATCHDOG_SETTINGS
 //------------------------
@@ -98,7 +96,6 @@ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
 #define PIOS_IRQ_PRIO_MID			8		// higher than RTOS
 #define PIOS_IRQ_PRIO_HIGH			5		// for SPI, ADC, I2C etc...
 #define PIOS_IRQ_PRIO_HIGHEST			4 		// for USART etc...
-
 //------------------------
 // PIOS_I2C
 // See also pios_board.c
@@ -155,6 +152,9 @@ extern uint32_t pios_com_debug_id;
 #define PIOS_COM_DEBUG                  (pios_com_debug_id)
 #endif	/* PIOS_INCLUDE_DEBUG_CONSOLE */
 
+extern uint32_t pios_com_hkosd_id;
+#define PIOS_COM_OSDHK              (pios_com_hkosd_id)
+
 //-------------------------
 // ADC
 // PIOS_ADC_PinGet(0) = Gyro Z
@@ -264,7 +264,6 @@ extern uint32_t pios_com_debug_id;
 #define PIOS_GPIO_CLKS				{  }
 #define PIOS_GPIO_NUM				0
 
-
 //-------------------------
 // USB
 //-------------------------

flight/targets/boards/osd/board-info.mk

@@ -13,16 +13,37 @@ OPENOCD_JTAG_CONFIG := stlink-v2.cfg
 OPENOCD_CONFIG      := stm32f4xx.stlink.cfg
 #OPENOCD_CONFIG      := stm32f4xx.cfg
 
+# Flash memory map for OSD:
+# Sector	start			size	use
+# 0			0x0800 0000		16k		BL
+# 1			0x0800 4000		16k		BL
+# 2			0x0800 8000		16k		EE
+# 3			0x0800 C000		16k		EE
+# 4			0x0801 0000		64k		Unused
+# 5			0x0802 0000		128k	FW
+# 6			0x0804 0000		128k	FW
+# 7 		0x0806 0000		128k	FW
+# 8 		0x0808 0000		128k	Unused
+# ..								..
+# 11		0x080E 0000		128k   	Unused
+
 # Note: These must match the values in link_$(BOARD)_memory.ld
 BL_BANK_BASE        := 0x08000000  # Start of bootloader flash
 BL_BANK_SIZE        := 0x00008000  # Should include BD_INFO region
 
-# Leave the remaining 16KB and 64KB sectors for other uses
+
+# Leave the remaining 16KB for settings storage
+
+EE_BANK_BASE        := 0x08008000  # EEPROM storage area
+EE_BANK_SIZE        := 0x00008000  # Size of EEPROM storage area
+
+# Leave the reamaining 64KB sectors for other uses
 
 FW_BANK_BASE        := 0x08020000  # Start of firmware flash
-FW_BANK_SIZE        := 0x00040000  # Should include FW_DESC_SIZE
+FW_BANK_SIZE        := 0x00060000  # Should include FW_DESC_SIZE
+
 
 FW_DESC_SIZE        := 0x00000064
 
 OSCILLATOR_FREQ     := 8000000
-SYSCLK_FREQ         := 108000000
+SYSCLK_FREQ         := 168000000

flight/targets/boards/osd/board_hw_defs.c

@@ -33,9 +33,9 @@
 static const struct pios_led pios_leds[] = {
 	[PIOS_LED_HEARTBEAT] = {
 		.pin = {
-			.gpio = GPIOC,
+			.gpio = GPIOB,
 			.init = {
-				.GPIO_Pin   = GPIO_Pin_5,
+				.GPIO_Pin   = GPIO_Pin_0,
 				.GPIO_Speed = GPIO_Speed_50MHz,
 				.GPIO_Mode  = GPIO_Mode_OUT,
 				.GPIO_OType = GPIO_OType_PP,
@@ -47,7 +47,7 @@ static const struct pios_led pios_leds[] = {
 		.pin = {
 			.gpio = GPIOC,
 			.init = {
-				.GPIO_Pin   = GPIO_Pin_4,
+				.GPIO_Pin   = GPIO_Pin_5,
 				.GPIO_Speed = GPIO_Speed_50MHz,
 				.GPIO_Mode  = GPIO_Mode_OUT,
 				.GPIO_OType = GPIO_OType_PP,
@@ -69,6 +69,30 @@ const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revisio
 
 #endif	/* PIOS_INCLUDE_LED */
 
+
+#if defined(PIOS_INCLUDE_FLASH)
+#include "pios_flashfs_logfs_priv.h"
+#include "pios_flash_internal_priv.h"
+
+static const struct pios_flash_internal_cfg flash_internal_cfg = {
+};
+
+static const struct flashfs_logfs_cfg flashfs_internal_cfg = {
+.fs_magic = 0x99abcfef,
+.total_fs_size = EE_BANK_SIZE, /* 32K bytes (2x16KB sectors) */
+.arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 1 sector */
+.slot_size = 0x00000100, /* 256 bytes */
+
+.start_offset = EE_BANK_BASE, /* start after the bootloader */
+.sector_size = 0x00004000, /* 16K bytes */
+.page_size = 0x00004000, /* 16K bytes */
+};
+
+#include "pios_flash.h"
+
+#endif /* PIOS_INCLUDE_FLASH */
+
+
 #if defined(PIOS_INCLUDE_SPI)
 
 #include <pios_spi_priv.h>
@@ -200,6 +224,7 @@ void PIOS_SPI_sdcard_irq_handler(void)
 #include <pios_usart_priv.h>
 
 #if defined(PIOS_INCLUDE_GPS)
+
 /*
  * GPS USART
  */
@@ -244,7 +269,6 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = {
 		},
 	},
 };
-
 #endif /* PIOS_INCLUDE_GPS */
 
 #ifdef PIOS_INCLUDE_COM_AUX
@@ -519,44 +543,91 @@ const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = {
 
 #if defined(PIOS_INCLUDE_VIDEO)
 
-#include <pios_video.h>
+static const TIM_TimeBaseInitTypeDef tim_8_time_base = {
-static const struct pios_exti_cfg pios_exti_hsync_cfg __exti_config = {
+	.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1,
-	.vector = PIOS_Hsync_ISR,
+	.TIM_ClockDivision = TIM_CKD_DIV1,
-	.line = EXTI_Line2,
+	.TIM_CounterMode = TIM_CounterMode_Up,
-	.pin = {
+	.TIM_Period = ((1000000 / 50000) - 1),
-		.gpio = GPIOD,
+	.TIM_RepetitionCounter = 0x0000,
-		.init = {
+};
-			.GPIO_Pin = GPIO_Pin_2,
+
-			.GPIO_Speed = GPIO_Speed_100MHz,
+static const struct pios_tim_clock_cfg tim_8_cfg = {
-			.GPIO_Mode = GPIO_Mode_IN,
+	.timer = TIM8,
-			.GPIO_OType = GPIO_OType_OD,
+	.time_base_init = &tim_8_time_base,
-			.GPIO_PuPd = GPIO_PuPd_NOPULL,
-		},
-	},
 	.irq = {
 		.init = {
-			.NVIC_IRQChannel = EXTI2_IRQn,
+			.NVIC_IRQChannel                   = TIM8_CC_IRQn,
-			.NVIC_IRQChannelPreemptionPriority = 0,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
 			.NVIC_IRQChannelSubPriority        = 0,
 			.NVIC_IRQChannelCmd                = ENABLE,
 		},
 	},
-	.exti = {
-		.init = {
-			.EXTI_Line = EXTI_Line2, // matches above GPIO pin
-			.EXTI_Mode = EXTI_Mode_Interrupt,
-			.EXTI_Trigger = EXTI_Trigger_Rising_Falling,
-			.EXTI_LineCmd = ENABLE,
-		},
-	},
 };
-static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = {
+
-		.vector = PIOS_Vsync_ISR,
+/**
-		.line = EXTI_Line11,
+ * Pios servo configuration structures
+ */
+#include <pios_servo_priv.h>
+static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
+		{
+			.timer = TIM8,
+			.timer_chan = TIM_Channel_3,
 			.pin = {
 				.gpio = GPIOC,
 				.init = {
-				.GPIO_Pin = GPIO_Pin_11,
+					.GPIO_Pin = GPIO_Pin_8,
+					.GPIO_Speed = GPIO_Speed_2MHz,
+					.GPIO_Mode  = GPIO_Mode_AF,
+					.GPIO_OType = GPIO_OType_PP,
+					.GPIO_PuPd  = GPIO_PuPd_UP
+				},
+				.pin_source = GPIO_PinSource8,
+			},
+			.remap = GPIO_AF_TIM8,
+		},
+		{
+			.timer = TIM8,
+			.timer_chan = TIM_Channel_4,
+			.pin = {
+				.gpio = GPIOC,
+				.init = {
+					.GPIO_Pin = GPIO_Pin_9,
+					.GPIO_Speed = GPIO_Speed_2MHz,
+					.GPIO_Mode  = GPIO_Mode_AF,
+					.GPIO_OType = GPIO_OType_PP,
+					.GPIO_PuPd  = GPIO_PuPd_UP
+				},
+				.pin_source = GPIO_PinSource9,
+			},
+			.remap = GPIO_AF_TIM8,
+		},
+};
+
+const struct pios_servo_cfg pios_servo_cfg = {
+	.tim_oc_init = {
+		.TIM_OCMode = TIM_OCMode_PWM1,
+		.TIM_OutputState = TIM_OutputState_Enable,
+		.TIM_OutputNState = TIM_OutputNState_Disable,
+		.TIM_Pulse = 0,
+		.TIM_OCPolarity = TIM_OCPolarity_High,
+		.TIM_OCNPolarity = TIM_OCPolarity_High,
+		.TIM_OCIdleState = TIM_OCIdleState_Reset,
+		.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
+	},
+	.channels = pios_tim_servoport_all_pins,
+	.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
+};
+
+
+
+#include <pios_video.h>
+static const struct pios_exti_cfg pios_exti_hsync_cfg __exti_config = {
+	.vector = PIOS_Hsync_ISR,
+	.line = EXTI_Line7,
+	.pin = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_7,
 			.GPIO_Speed = GPIO_Speed_100MHz,
 			.GPIO_Mode = GPIO_Mode_IN,
 			.GPIO_OType = GPIO_OType_OD,
@@ -565,7 +636,7 @@ static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = {
 	},
 	.irq = {
 		.init = {
-				.NVIC_IRQChannel = EXTI15_10_IRQn,
+			.NVIC_IRQChannel = EXTI9_5_IRQn,
 			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
 			.NVIC_IRQChannelSubPriority = 0,
 			.NVIC_IRQChannelCmd = ENABLE,
@@ -573,7 +644,39 @@ static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = {
 	},
 	.exti = {
 		.init = {
-				.EXTI_Line = EXTI_Line11, // matches above GPIO pin
+			.EXTI_Line = EXTI_Line7, // matches above GPIO pin
+			.EXTI_Mode = EXTI_Mode_Interrupt,
+			//.EXTI_Trigger = EXTI_Trigger_Rising_Falling,
+			.EXTI_Trigger = EXTI_Trigger_Falling,
+			//.EXTI_Trigger = EXTI_Trigger_Rising,
+			.EXTI_LineCmd = ENABLE,
+		},
+	},
+};
+static const struct pios_exti_cfg pios_exti_vsync_cfg __exti_config = {
+		.vector = PIOS_Vsync_ISR,
+		.line = EXTI_Line5,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_5,
+				.GPIO_Speed = GPIO_Speed_100MHz,
+				.GPIO_Mode = GPIO_Mode_IN,
+				.GPIO_OType = GPIO_OType_OD,
+				.GPIO_PuPd = GPIO_PuPd_NOPULL,
+			},
+		},
+		.irq = {
+			.init = {
+				.NVIC_IRQChannel = EXTI9_5_IRQn,
+				.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+				.NVIC_IRQChannelSubPriority = 0,
+				.NVIC_IRQChannelCmd = ENABLE,
+			},
+		},
+		.exti = {
+			.init = {
+				.EXTI_Line = EXTI_Line5, // matches above GPIO pin
 				.EXTI_Mode = EXTI_Mode_Interrupt,
 				.EXTI_Trigger = EXTI_Trigger_Falling,
 				.EXTI_LineCmd = ENABLE,
@@ -587,7 +690,7 @@ static const struct pios_video_cfg pios_video_cfg = {
 		.regs = SPI3,
 		.remap = GPIO_AF_SPI3,
 		.init = {
-			.SPI_Mode              = SPI_Mode_Master,
+			.SPI_Mode              = SPI_Mode_Slave,
 			.SPI_Direction         = SPI_Direction_1Line_Tx,
 			.SPI_DataSize          = SPI_DataSize_8b,
 			.SPI_NSS               = SPI_NSS_Soft,
@@ -595,7 +698,7 @@ static const struct pios_video_cfg pios_video_cfg = {
 			.SPI_CRCPolynomial     = 7,
 			.SPI_CPOL              = SPI_CPOL_Low,
 			.SPI_CPHA              = SPI_CPHA_2Edge,
-			.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4,
+			.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
 		},
 		.use_crc = false,
 		.dma = {
@@ -604,12 +707,11 @@ static const struct pios_video_cfg pios_video_cfg = {
 				.flags = (DMA_IT_TCIF7),
 				.init = {
 					.NVIC_IRQChannel = DMA1_Stream7_IRQn,
-					.NVIC_IRQChannelPreemptionPriority = 0,
+					.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
 					.NVIC_IRQChannelSubPriority = 0,
 					.NVIC_IRQChannelCmd = ENABLE,
 				},
 			},
-
 			.rx = {},
 			.tx = {
 				.channel = DMA1_Stream7,
@@ -624,9 +726,9 @@ static const struct pios_video_cfg pios_video_cfg = {
 					.DMA_MemoryDataSize     = DMA_MemoryDataSize_Word,
 					.DMA_Mode               = DMA_Mode_Normal,
 					.DMA_Priority           = DMA_Priority_VeryHigh,
-					.DMA_FIFOMode           = DMA_FIFOMode_Disable,
+					.DMA_FIFOMode           = DMA_FIFOMode_Enable,
 					.DMA_FIFOThreshold      = DMA_FIFOThreshold_Full,
-					.DMA_MemoryBurst        = DMA_MemoryBurst_Single,
+					.DMA_MemoryBurst        = DMA_MemoryBurst_INC4,
 					.DMA_PeripheralBurst    = DMA_PeripheralBurst_Single,
 				},
 			},
@@ -651,16 +753,7 @@ static const struct pios_video_cfg pios_video_cfg = {
 				.GPIO_PuPd = GPIO_PuPd_NOPULL
 			},
 		},
-		.mosi = {
+		.mosi = {},
-			.gpio = GPIOC,
-			.init = {
-				.GPIO_Pin = GPIO_Pin_12,
-				.GPIO_Speed = GPIO_Speed_50MHz,
-				.GPIO_Mode = GPIO_Mode_AF,
-				.GPIO_OType = GPIO_OType_PP,
-				.GPIO_PuPd = GPIO_PuPd_NOPULL
-			},
-		},
 		.slave_count = 1,
 	},
 	.level = {
@@ -683,12 +776,11 @@ static const struct pios_video_cfg pios_video_cfg = {
 					.flags   = (DMA_IT_TCIF5),
 					.init    = {
 						.NVIC_IRQChannel                   = DMA2_Stream5_IRQn,
-						.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+						.NVIC_IRQChannelPreemptionPriority = 0,
 						.NVIC_IRQChannelSubPriority        = 0,
 						.NVIC_IRQChannelCmd                = ENABLE,
 					},
 				},
-
 				.rx = {},
 				.tx = {
 					.channel = DMA2_Stream5,
@@ -703,9 +795,9 @@ static const struct pios_video_cfg pios_video_cfg = {
 						.DMA_MemoryDataSize     = DMA_MemoryDataSize_Word,
 						.DMA_Mode               = DMA_Mode_Normal,
 						.DMA_Priority           = DMA_Priority_VeryHigh,
-						.DMA_FIFOMode           = DMA_FIFOMode_Disable,
+						.DMA_FIFOMode           = DMA_FIFOMode_Enable,
 						.DMA_FIFOThreshold      = DMA_FIFOThreshold_Full,
-		                .DMA_MemoryBurst        = DMA_MemoryBurst_Single,
+		                .DMA_MemoryBurst        = DMA_MemoryBurst_INC4,
 		                .DMA_PeripheralBurst    = DMA_PeripheralBurst_Single,
 					},
 				},
@@ -730,23 +822,56 @@ static const struct pios_video_cfg pios_video_cfg = {
 					.GPIO_PuPd = GPIO_PuPd_UP
 				},
 			},
-			.mosi = {
+			.mosi = {},
+			.slave_count = 1,
+
+	},
+
+	.hsync = &pios_exti_hsync_cfg,
+	.vsync = &pios_exti_vsync_cfg,
+	
+	.pixel_timer = {
+		.timer = TIM4,
+		.timer_chan = TIM_Channel_1,
+		.pin = {
 			.gpio = GPIOB,
 			.init = {
-					.GPIO_Pin   = GPIO_Pin_5,
+				.GPIO_Pin = GPIO_Pin_6,
-					.GPIO_Speed = GPIO_Speed_50MHz,
+				.GPIO_Speed = GPIO_Speed_100MHz,
 				.GPIO_Mode  = GPIO_Mode_AF,
 				.GPIO_OType = GPIO_OType_PP,
 				.GPIO_PuPd  = GPIO_PuPd_UP
 			},
+			.pin_source = GPIO_PinSource6,
 		},
-			.slave_count = 1,
+		.remap = GPIO_AF_TIM4,
-
+	},
+	.hsync_capture = {
+		.timer = TIM4,
+		.timer_chan = TIM_Channel_2,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_7,
+				.GPIO_Speed = GPIO_Speed_100MHz,
+				.GPIO_Mode  = GPIO_Mode_AF,
+				.GPIO_OType = GPIO_OType_PP,
+				.GPIO_PuPd  = GPIO_PuPd_UP
+			},
+			.pin_source = GPIO_PinSource7,
+		},
+		.remap = GPIO_AF_TIM4,
+	},
+	.tim_oc_init = {
+		.TIM_OCMode = TIM_OCMode_PWM1,
+		.TIM_OutputState = TIM_OutputState_Enable,
+		.TIM_OutputNState = TIM_OutputNState_Disable,
+		.TIM_Pulse = 1,
+		.TIM_OCPolarity = TIM_OCPolarity_High,
+		.TIM_OCNPolarity = TIM_OCPolarity_High,
+		.TIM_OCIdleState = TIM_OCIdleState_Reset,
+		.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
 	},
-	/////////////////
-
-	.hsync = &pios_exti_hsync_cfg,
-	.vsync = &pios_exti_vsync_cfg,
 };
 
 #endif

flight/targets/boards/osd/firmware/Makefile

@@ -37,6 +37,9 @@ MODULES += Telemetry
 
 OPTMODULES =
 
+# Some diagnostics
+CDEFS += -DDIAG_TASKS
+
 # Misc options
 CFLAGS += -ffast-math
 
@@ -97,6 +100,9 @@ else
     SRC += $(OPTESTS)/$(TESTAPP).c
 endif
 
+CDEFS += -DEE_BANK_BASE=$(EE_BANK_BASE)
+CDEFS += -DEE_BANK_SIZE=$(EE_BANK_SIZE)
+
 	
 # Optional component libraries
 include $(PIOS)/common/libraries/dosfs/library.mk

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

@@ -99,14 +99,16 @@
 /* PIOS common peripherals */
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_IAP
-/* #define PIOS_INCLUDE_SERVO */
+#define PIOS_INCLUDE_SERVO
 /* #define PIOS_INCLUDE_I2C_ESC */
 /* #define PIOS_INCLUDE_OVERO */
 /* #define PIOS_OVERO_SPI */
 #define PIOS_INCLUDE_SDCARD
+/* #define PIOS_USE_SETTINGS_ON_SDCARD */
 #define LOG_FILENAME "startup.log"
-/* #define PIOS_INCLUDE_FLASH */
+#define PIOS_INCLUDE_FLASH
-/* #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS */
+#define PIOS_INCLUDE_FLASH_INTERNAL
+#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
 /* #define FLASH_FREERTOS */
 /* #define PIOS_INCLUDE_FLASH_EEPROM */
 

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

@@ -85,8 +85,8 @@ void PIOS_ADC_DMC_irq_handler(void)
 static void Clock(uint32_t spektrum_id);
 
 
-#define PIOS_COM_TELEM_RF_RX_BUF_LEN 512
+#define PIOS_COM_TELEM_RF_RX_BUF_LEN 128
-#define PIOS_COM_TELEM_RF_TX_BUF_LEN 512
+#define PIOS_COM_TELEM_RF_TX_BUF_LEN 128
 
 #define PIOS_COM_AUX_RX_BUF_LEN 512
 #define PIOS_COM_AUX_TX_BUF_LEN 512
@@ -104,7 +104,42 @@ uint32_t pios_com_gps_id;
 uint32_t pios_com_telem_usb_id;
 uint32_t pios_com_telem_rf_id;
 
+/**
+ * TIM3 is triggered by the HSYNC signal into its ETR line and will divide the 
+ *  APB1_CLOCK to generate a pixel clock that is used by the SPI CLK lines.
+ * TIM4 will be synced to it and will divide by that times the pixel width to
+ *  fire an IRQ when the last pixel of the line has been output.  Then the timer will
+ *  be rearmed and wait for the next HSYNC signal.  
+ * The critical timing detail is that the task be _DISABLED_ at the end of the line
+ *  before an extra pixel is clocked out
+ *  or we will need to configure the DMA task per line
+ */
+#include "pios_tim_priv.h"
+#define NTSC_PX_CLOCK  6797088
+#define PAL_PX_CLOCK   6750130
+#define PX_PERIOD      ((PIOS_PERIPHERAL_APB1_CLOCK / NTSC_PX_CLOCK) + 1)
+#define LINE_PERIOD    PX_PERIOD * GRAPHICS_WIDTH
 
+static const TIM_TimeBaseInitTypeDef tim_4_time_base = {
+	.TIM_Prescaler = 0, //PIOS_PERIPHERAL_APB1_CLOCK,
+	.TIM_ClockDivision = TIM_CKD_DIV1,
+	.TIM_CounterMode = TIM_CounterMode_Up,
+	.TIM_Period = LINE_PERIOD - 1,
+	.TIM_RepetitionCounter = 0x0000,
+};
+
+const static struct pios_tim_clock_cfg pios_tim4_cfg = {
+	.timer = TIM4,
+	.time_base_init = &tim_4_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM4_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	}
+};
 
 void PIOS_Board_Init(void) {
 
@@ -119,17 +154,34 @@ void PIOS_Board_Init(void) {
 		PIOS_Assert(0);
 	}
 
+#if defined(PIOS_INCLUDE_SDCARD)
 	/* Enable and mount the SDCard */
 	PIOS_SDCARD_Init(pios_spi_sdcard_id);
 	PIOS_SDCARD_MountFS(0);
+#endif
 #endif /* PIOS_INCLUDE_SPI */
 
+#ifdef PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
+uintptr_t flash_id;
+uintptr_t fs_id;
+PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg);
+PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id);
+#elif !defined(PIOS_USE_SETTINGS_ON_SDCARD)
+#error No setting storage specified. (define PIOS_USE_SETTINGS_ON_SDCARD or INCLUDE_FLASH_SECTOR_SETTINGS)
+#endif
+
+
 	/* Initialize UAVObject libraries */
 	EventDispatcherInitialize();
 	UAVObjInitialize();
 
 	HwSettingsInitialize();
 
+#ifdef PIOS_INCLUDE_WDG
+	/* Initialize watchdog as early as possible to catch faults during init */
+	PIOS_WDG_Init();
+#endif /* PIOS_INCLUDE_WDG */
+
 	/* Initialize the alarms library */
 	AlarmsInitialize();
 
@@ -151,6 +203,7 @@ void PIOS_Board_Init(void) {
 		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
 	}
 
+
 #if defined(PIOS_INCLUDE_RTC)
 	/* Initialize the real-time clock and its associated tick */
 	PIOS_RTC_Init(&pios_rtc_main_cfg);
@@ -406,6 +459,10 @@ void PIOS_Board_Init(void) {
 #endif
 
 #if defined(PIOS_INCLUDE_VIDEO)
+	PIOS_TIM_InitClock(&tim_8_cfg);
+	PIOS_Servo_Init(&pios_servo_cfg);
+	// Start the pixel and line clock counter
+	//PIOS_TIM_InitClock(&pios_tim4_cfg);
 	PIOS_Video_Init(&pios_video_cfg);
 #endif
 }

flight/targets/boards/osd/pios_board.h

@@ -1,4 +1,4 @@
- /**
+/**
  ******************************************************************************
  *
  * @file       pios_board.h
@@ -32,14 +32,14 @@
 // Timers and Channels Used
 
 /*
-Timer | Channel 1  | Channel 2  | Channel 3  | Channel 4
+ Timer | Channel 1  | Channel 2  | Channel 3  | Channel 4
-------+------------+------------+------------+------------
+ ------+------------+------------+------------+------------
-TIM1  |                       DELAY                      |
+ TIM1  |                       DELAY                      |
-TIM2  |                         | PPM Output | PPM Input |
+ TIM2  |                         | PPM Output | PPM Input |
-TIM3  |                  TIMER INTERRUPT                 |
+ TIM3  |                  TIMER INTERRUPT                 |
-TIM4  |                     STOPWATCH                    |
+ TIM4  |                     STOPWATCH                    |
-------+------------+------------+------------+------------
+ ------+------------+------------+------------+------------
-*/
+ */
 
 //------------------------
 // DMA Channels Used
@@ -75,7 +75,7 @@ TIM4  |                     STOPWATCH                    |
 //#define PIOS_PERIPHERAL_CLOCK
 //#define PIOS_PERIPHERAL_CLOCK
 
-#define PIOS_SYSCLK										108000000
+#define PIOS_SYSCLK										168000000
 //	Peripherals that belongs to APB1 are:
 //	DAC			|PWR				|CAN1,2
 //	I2C1,2,3		|UART4,5			|USART3,2
@@ -98,19 +98,12 @@ TIM4  |                     STOPWATCH                    |
 //
 #define PIOS_PERIPHERAL_APB2_CLOCK					PIOS_SYSCLK
 
-
 //------------------------
 // TELEMETRY
 //------------------------
 #define TELEM_QUEUE_SIZE         20
 #define PIOS_TELEM_STACK_SIZE    624
 
-// *****************************************************************
-// System Settings
-
-#define PIOS_MASTER_CLOCK                       108000000ul
-#define PIOS_PERIPHERAL_CLOCK                   (PIOS_MASTER_CLOCK / 2)
-
 // *****************************************************************
 // Interrupt Priorities
 
@@ -119,7 +112,6 @@ TIM4  |                     STOPWATCH                    |
 #define PIOS_IRQ_PRIO_HIGH			5		// for SPI, ADC, I2C etc...
 #define PIOS_IRQ_PRIO_HIGHEST		4 		// for USART etc...
 
-
 //------------------------
 // WATCHDOG_SETTINGS
 //------------------------
@@ -129,7 +121,7 @@ TIM4  |                     STOPWATCH                    |
 #define PIOS_WDG_STABILIZATION   0x0002
 #define PIOS_WDG_ATTITUDE        0x0004
 #define PIOS_WDG_MANUAL          0x0008
-
+#define PIOS_WDG_OSDGEN          0x0010
 
 // *****************************************************************
 // PIOS_LED
@@ -148,13 +140,13 @@ TIM4  |                     STOPWATCH                    |
 // Timer interrupt
 
 /*#define TIMER_INT_TIMER					TIM3
-#define TIMER_INT_FUNC					TIM3_IRQHandler
+ #define TIMER_INT_FUNC					TIM3_IRQHandler
-#define TIMER_INT_PRIORITY				2
+ #define TIMER_INT_PRIORITY				2
 
-// *****************************************************************
+ // *****************************************************************
-// Stop watch timer
+ // Stop watch timer
 
-#define STOPWATCH_TIMER					TIM4*/
+ #define STOPWATCH_TIMER					TIM4*/
 
 //------------------------
 // PIOS_SPI
@@ -195,7 +187,6 @@ extern uint32_t pios_com_telem_usb_id;
 //extern uint32_t pios_com_gps_id;
 //#define PIOS_COM_GPS                    (pios_com_gps_id)
 
-
 #if defined(PIOS_INCLUDE_USB_HID)
 extern uint32_t pios_com_telem_usb_id;
 #define PIOS_COM_TELEM_USB              (pios_com_telem_usb_id)
@@ -206,12 +197,13 @@ extern uint32_t pios_com_telem_usb_id;
 
 //-------------------------
 // ADC
-// PIOS_ADC_PinGet(0) = External voltage
+// PIOS_ADC_PinGet(0) = Current
-// PIOS_ADC_PinGet(1) = AUX1 (PX2IO external pressure port)
+// PIOS_ADC_PinGet(1) = Voltage
-// PIOS_ADC_PinGet(2) = AUX2 (Current sensor, if available)
+// PIOS_ADC_PinGet(2) = Flight
-// PIOS_ADC_PinGet(3) = AUX3
+// PIOS_ADC_PinGet(3) = Temperature sensor
-// PIOS_ADC_PinGet(4) = VREF
+// PIOS_ADC_PinGet(4) = Video
-// PIOS_ADC_PinGet(5) = Temperature sensor
+// PIOS_ADC_PinGet(5) = RSSI
+// PIOS_ADC_PinGet(6) = VREF
 //-------------------------
 
 #define PIOS_DMA_PIN_CONFIG \
@@ -219,10 +211,10 @@ extern uint32_t pios_com_telem_usb_id;
 {GPIOC, GPIO_Pin_0, ADC_Channel_10}, \
 {GPIOC, GPIO_Pin_1, ADC_Channel_11}, \
 {GPIOC, GPIO_Pin_2, ADC_Channel_12}, \
+{NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */\
 {GPIOC, GPIO_Pin_3, ADC_Channel_13}, \
 {GPIOA, GPIO_Pin_7, ADC_Channel_7}, \
-{NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */\
+{NULL, 0, ADC_Channel_Vrefint} /* Voltage reference */\
-{NULL, 0, ADC_Channel_TempSensor} /* Temperature sensor */\
 }
 
 /* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
@@ -231,21 +223,21 @@ extern uint32_t pios_com_telem_usb_id;
 #define PIOS_ADC_NUM_CHANNELS 7
 #define PIOS_ADC_MAX_OVERSAMPLING 10
 #define PIOS_ADC_USE_ADC2 0
+#define PIOS_ADC_USE_TEMP_SENSOR 1
 
 // *****************************************************************
 // USB
 
 #if defined(PIOS_INCLUDE_USB_HID)
-	#define PIOS_USB_ENABLED				1
+#define PIOS_USB_ENABLED				1
-	#define PIOS_USB_DETECT_GPIO_PORT		GPIO_IN_2_PORT
+#define PIOS_USB_DETECT_GPIO_PORT		GPIO_IN_2_PORT
-	#define PIOS_USB_DETECT_GPIO_PIN		GPIO_IN_2_PIN
+#define PIOS_USB_DETECT_GPIO_PIN		GPIO_IN_2_PIN
-	#define PIOS_USB_DETECT_EXTI_LINE		EXTI_Line4
+#define PIOS_USB_DETECT_EXTI_LINE		EXTI_Line4
-	#define PIOS_IRQ_USB_PRIORITY			8
+#define PIOS_IRQ_USB_PRIORITY			8
-        #define PIOS_USB_RX_BUFFER_SIZE                 512
+#define PIOS_USB_RX_BUFFER_SIZE                 512
-        #define PIOS_USB_TX_BUFFER_SIZE                 512
+#define PIOS_USB_TX_BUFFER_SIZE                 512
 #endif
 
-
 // *****************************************************************
 //--------------------------
 // Timer controller settings
@@ -274,7 +266,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id;
 //------------------------
 #define PIOS_BMP085_OVERSAMPLING                3
 
-
 /**
  * glue macros for file IO
  * STM32 uses DOSFS for file IO

flight/targets/boards/revolution/board_hw_defs.c

@@ -967,6 +967,91 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
 
 #endif	/* PIOS_INCLUDE_DSM */
 
+/*
+ * HK OSD
+ */
+static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
+	.regs = USART1,
+	.remap = GPIO_AF_USART1,
+	.init = {
+		.USART_BaudRate            = 57600,
+		.USART_WordLength          = USART_WordLength_8b,
+		.USART_Parity              = USART_Parity_No,
+		.USART_StopBits            = USART_StopBits_1,
+		.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
+		.USART_Mode                = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = USART1_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_10,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_9,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
+static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
+	.regs = USART3,
+	.remap = GPIO_AF_USART3,
+	.init = {
+		.USART_BaudRate            = 57600,
+		.USART_WordLength          = USART_WordLength_8b,
+		.USART_Parity              = USART_Parity_No,
+		.USART_StopBits            = USART_StopBits_1,
+		.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
+		.USART_Mode                = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = USART3_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_11,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_10,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
 #if defined(PIOS_INCLUDE_COM)
 
 #include <pios_com_priv.h>

flight/targets/boards/revolution/firmware/Makefile

@@ -34,6 +34,7 @@ MODULES += Altitude/revolution
 MODULES += Airspeed/revolution
 MODULES += AltitudeHold
 MODULES += Stabilization
+MODULES += VtolPathFollower
 MODULES += ManualControl
 MODULES += Actuator
 MODULES += GPS
@@ -44,8 +45,8 @@ MODULES += FirmwareIAP
 MODULES += Radio
 MODULES += PathPlanner
 MODULES += FixedWingPathFollower
+MODULES += Osd/osdoutout
 MODULES += Telemetry
-#MODULES += VtolPathFollower  ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
 
 OPTMODULES =
 

flight/targets/boards/revolution/firmware/UAVObjects.inc

@@ -89,6 +89,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudeholddesired
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
+UAVOBJSRCFILENAMES += poilocation
+UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
 

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

@@ -217,6 +217,9 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 #define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512
 #define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512
 
+#define PIOS_COM_HKOSD_RX_BUF_LEN 22
+#define PIOS_COM_HKOSD_TX_BUF_LEN 22
+
 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
 #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
 uint32_t pios_com_debug_id;
@@ -227,6 +230,7 @@ uint32_t pios_com_telem_usb_id = 0;
 uint32_t pios_com_telem_rf_id = 0;
 uint32_t pios_com_bridge_id = 0;
 uint32_t pios_com_overo_id = 0;
+uint32_t pios_com_hkosd_id = 0;
 #if defined(PIOS_INCLUDE_RFM22B)
 uint32_t pios_rfm22b_id = 0;
 #endif
@@ -576,7 +580,9 @@ void PIOS_Board_Init(void) {
 		case HWSETTINGS_RM_MAINPORT_COMBRIDGE:
 			PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
 			break;
-			
+		case HWSETTINGS_RM_MAINPORT_OSDHK:
+			PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
+			break;
 	} /* 	hwsettings_rm_mainport */
 
 	if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) {
@@ -639,7 +645,11 @@ void PIOS_Board_Init(void) {
 		case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE:
 			PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
 			break;
-	} /* hwsettings_rv_flexiport */
+		case HWSETTINGS_RM_FLEXIPORT_OSDHK:
+			PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
+			break;
+	} /* hwsettings_rm_flexiport */
+
 
 	/* Initalize the RFM22B radio COM device. */
 #if defined(PIOS_INCLUDE_RFM22B)

flight/targets/boards/revolution/pios_board.h

@@ -35,18 +35,18 @@
 // Timers and Channels Used
 //------------------------
 /*
-Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
+ Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
-------+-----------+-----------+-----------+----------
+ ------+-----------+-----------+-----------+----------
-TIM1  |           |           |           |
+ TIM1  |           |           |           |
-TIM2  | --------------- PIOS_DELAY -----------------
+ TIM2  | --------------- PIOS_DELAY -----------------
-TIM3  |           |           |           |
+ TIM3  |           |           |           |
-TIM4  |           |           |           |
+ TIM4  |           |           |           |
-TIM5  |           |           |           |
+ TIM5  |           |           |           |
-TIM6  |           |           |           |
+ TIM6  |           |           |           |
-TIM7  |           |           |           |
+ TIM7  |           |           |           |
-TIM8  |           |           |           |
+ TIM8  |           |           |           |
-------+-----------+-----------+-----------+----------
+ ------+-----------+-----------+-----------+----------
-*/
+ */
 
 //------------------------
 // DMA Channels Used
@@ -71,7 +71,6 @@ TIM8  |           |           |           |
 #define BOARD_WRITABLE	true
 #define MAX_DEL_RETRYS	3
 
-
 //------------------------
 // PIOS_LED
 //------------------------
@@ -146,11 +145,13 @@ extern uint32_t pios_com_gps_id;
 extern uint32_t pios_com_telem_usb_id;
 extern uint32_t pios_com_bridge_id;
 extern uint32_t pios_com_vcp_id;
+extern uint32_t pios_com_hkosd_id;
 #define PIOS_COM_GPS                    (pios_com_gps_id)
 #define PIOS_COM_TELEM_USB              (pios_com_telem_usb_id)
 #define PIOS_COM_TELEM_RF               (pios_com_telem_rf_id)
 #define PIOS_COM_BRIDGE                 (pios_com_bridge_id)
 #define PIOS_COM_VCP                    (pios_com_vcp_id)
+#define PIOS_COM_OSDHK              (pios_com_hkosd_id)
 
 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
 extern uint32_t pios_com_debug_id;
@@ -220,7 +221,6 @@ extern uint32_t pios_packet_handler;
 #define PIOS_IRQ_PRIO_MID                       8               // higher than RTOS
 #define PIOS_IRQ_PRIO_HIGH                      5               // for SPI, ADC, I2C etc...
 #define PIOS_IRQ_PRIO_HIGHEST                   4               // for USART etc...
-
 //------------------------
 // PIOS_RCVR
 // See also pios_board.c
@@ -292,7 +292,8 @@ extern uint32_t pios_packet_handler;
 #define PIOS_ADC_NUM_CHANNELS           4
 #define PIOS_ADC_MAX_OVERSAMPLING       2
 #define PIOS_ADC_USE_ADC2               0
-#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0
+#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f
+#define PIOS_ADC_USE_TEMP_SENSOR        1
 
 //-------------------------
 // USB

flight/targets/boards/revoproto/board_hw_defs.c

@@ -1068,6 +1068,91 @@ static const struct pios_sbus_cfg pios_sbus_cfg = {
 
 #endif	/* PIOS_INCLUDE_SBUS */
 
+/*
+ * HK OSD
+ */
+static const struct pios_usart_cfg pios_usart_hkosd_auxsbus_cfg = {
+	.regs = UART4,
+	.remap = GPIO_AF_UART4,
+	.init = {
+		.USART_BaudRate            = 57600,
+		.USART_WordLength          = USART_WordLength_8b,
+		.USART_Parity              = USART_Parity_No,
+		.USART_StopBits            = USART_StopBits_1,
+		.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
+		.USART_Mode                = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = UART4_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_1,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_0,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
+static const struct pios_usart_cfg pios_usart_hkosd_aux_cfg = {
+	.regs = USART6,
+	.remap = GPIO_AF_USART6,
+	.init = {
+		.USART_BaudRate = 57600,
+		.USART_WordLength = USART_WordLength_8b,
+		.USART_Parity = USART_Parity_No,
+		.USART_StopBits = USART_StopBits_1,
+		.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
+		.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = USART6_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_7,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_6,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
 #if defined(PIOS_INCLUDE_COM)
 
 #include <pios_com_priv.h>
@@ -1353,7 +1438,15 @@ static const TIM_TimeBaseInitTypeDef tim_1_time_base = {
 	.TIM_RepetitionCounter = 0x0000,
 };
 
-// Set up timers that only have inputs on APB2
+static const TIM_TimeBaseInitTypeDef tim_8_time_base = {
+	.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / 1000000) - 1,
+	.TIM_ClockDivision = TIM_CKD_DIV1,
+	.TIM_CounterMode = TIM_CounterMode_Up,
+	.TIM_Period = 0xFFFF,
+	.TIM_RepetitionCounter = 0x0000,
+};
+
+// Set up timers that only have inputs on APB1
 static const TIM_TimeBaseInitTypeDef tim_4_time_base = {
 	.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / 1000000) - 1,
 	.TIM_ClockDivision = TIM_CKD_DIV1,
@@ -1405,6 +1498,19 @@ static const struct pios_tim_clock_cfg tim_5_cfg = {
 	},
 };
 
+static const struct pios_tim_clock_cfg tim_8_cfg = {
+	.timer = TIM8,
+	.time_base_init = &tim_8_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM8_CC_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
 static const struct pios_tim_clock_cfg tim_9_cfg = {
 	.timer = TIM9,
 	.time_base_init = &tim_9_10_11_time_base,
@@ -1831,6 +1937,52 @@ static const struct pios_ppm_cfg pios_ppm_cfg = {
 #include "pios_rcvr_priv.h"
 #endif /* PIOS_INCLUDE_RCVR */
 
+/*
+ * SONAR Inputs
+ */
+#if defined(PIOS_INCLUDE_HCSR04)
+#include <pios_hcsr04_priv.h>
+
+static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = {
+{
+	.timer = TIM8,
+	.timer_chan = TIM_Channel_3,
+	.pin = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_8,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_PuPd  = GPIO_PuPd_DOWN
+		},
+		.pin_source = GPIO_PinSource8,
+	},
+	.remap = GPIO_AF_TIM8,
+},
+};
+
+const struct pios_hcsr04_cfg pios_hcsr04_cfg = {
+	.tim_ic_init = {
+		.TIM_ICPolarity = TIM_ICPolarity_Rising,
+		.TIM_ICSelection = TIM_ICSelection_DirectTI,
+		.TIM_ICPrescaler = TIM_ICPSC_DIV1,
+		.TIM_ICFilter = 0x0,
+	},
+	.channels = pios_tim_hcsr04_port_all_channels,
+	.num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels),
+	.trigger = {
+		.gpio = GPIOD,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_10,
+			.GPIO_Mode  = GPIO_Mode_OUT,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+		},
+	},
+};
+#endif
+
 #if defined(PIOS_INCLUDE_USB)
 #include "pios_usb_priv.h"
 

flight/targets/boards/revoproto/firmware/UAVObjects.inc

@@ -94,6 +94,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudeholddesired
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
+UAVOBJSRCFILENAMES += poilocation
+UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )

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

@@ -293,12 +293,17 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 #define PIOS_COM_AUX_RX_BUF_LEN 512
 #define PIOS_COM_AUX_TX_BUF_LEN 512
 
+#define PIOS_COM_HKOSD_RX_BUF_LEN 22
+#define PIOS_COM_HKOSD_TX_BUF_LEN 22
+
+
 uint32_t pios_com_aux_id = 0;
 uint32_t pios_com_gps_id = 0;
 uint32_t pios_com_telem_usb_id = 0;
 uint32_t pios_com_telem_rf_id = 0;
 uint32_t pios_com_bridge_id = 0;
 uint32_t pios_com_overo_id = 0;
+uint32_t pios_com_hkosd_id = 0;
 
 /* 
  * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only
@@ -638,6 +643,10 @@ void PIOS_Board_Init(void) {
 		case HWSETTINGS_RV_AUXPORT_COMBRIDGE:
 			PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
 			break;
+		case HWSETTINGS_RV_AUXPORT_OSDHK:
+			PIOS_Board_configure_com(&pios_usart_hkosd_aux_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
+			break;
+
 	} /* hwsettings_rv_auxport */
 	/* Configure AUXSbusPort */
 	//TODO: ensure that the serial invertion pin is setted correctly
@@ -700,6 +709,9 @@ void PIOS_Board_Init(void) {
 		case HWSETTINGS_RV_AUXSBUSPORT_COMBRIDGE:
 			PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
 			break;
+		case HWSETTINGS_RV_AUXSBUSPORT_OSDHK:
+			PIOS_Board_configure_com(&pios_usart_hkosd_auxsbus_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
+			break;
 	} /* hwsettings_rv_auxport */
 	
 	/* Configure FlexiPort */
@@ -818,6 +830,15 @@ void PIOS_Board_Init(void) {
 
 #endif
 
+#if defined(PIOS_INCLUDE_HCSR04)
+		{
+			PIOS_TIM_InitClock(&tim_8_cfg);
+			uint32_t pios_hcsr04_id;
+			PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg);
+		}
+#endif
+
+
 #if defined(PIOS_INCLUDE_GCSRCVR)
 	GCSReceiverInitialize();
 	uint32_t pios_gcsrcvr_id;

flight/targets/boards/revoproto/pios_board.h

@@ -35,18 +35,18 @@
 // Timers and Channels Used
 //------------------------
 /*
-Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
+ Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
-------+-----------+-----------+-----------+----------
+ ------+-----------+-----------+-----------+----------
-TIM1  |           |           |           |
+ TIM1  |           |           |           |
-TIM2  | --------------- PIOS_DELAY -----------------
+ TIM2  | --------------- PIOS_DELAY -----------------
-TIM3  |           |           |           |
+ TIM3  |           |           |           |
-TIM4  |           |           |           |
+ TIM4  |           |           |           |
-TIM5  |           |           |           |
+ TIM5  |           |           |           |
-TIM6  |           |           |           |
+ TIM6  |           |           |           |
-TIM7  |           |           |           |
+ TIM7  |           |           |           |
-TIM8  |           |           |           |
+ TIM8  |           |           |           |
-------+-----------+-----------+-----------+----------
+ ------+-----------+-----------+-----------+----------
-*/
+ */
 
 //------------------------
 // DMA Channels Used
@@ -71,7 +71,6 @@ TIM8  |           |           |           |
 #define BOARD_WRITABLE	true
 #define MAX_DEL_RETRYS	3
 
-
 //------------------------
 // PIOS_LED
 //------------------------
@@ -94,6 +93,7 @@ TIM8  |           |           |           |
 #define PIOS_WDG_ATTITUDE        0x0004
 #define PIOS_WDG_MANUAL          0x0008
 #define PIOS_WDG_SENSORS         0x0010
+#define PIOS_WDG_AUTOTUNE        0x0020
 
 //------------------------
 // PIOS_I2C
@@ -125,6 +125,7 @@ extern uint32_t pios_com_aux_id;
 extern uint32_t pios_com_telem_usb_id;
 extern uint32_t pios_com_bridge_id;
 extern uint32_t pios_com_vcp_id;
+extern uint32_t pios_com_hkosd_id;
 #define PIOS_COM_AUX                    (pios_com_aux_id)
 #define PIOS_COM_GPS                    (pios_com_gps_id)
 #define PIOS_COM_TELEM_USB              (pios_com_telem_usb_id)
@@ -132,6 +133,7 @@ extern uint32_t pios_com_vcp_id;
 #define PIOS_COM_BRIDGE                 (pios_com_bridge_id)
 #define PIOS_COM_VCP                    (pios_com_vcp_id)
 #define PIOS_COM_DEBUG                  PIOS_COM_AUX
+#define PIOS_COM_OSDHK              (pios_com_hkosd_id)
 
 //------------------------
 // TELEMETRY 
@@ -173,7 +175,6 @@ extern uint32_t pios_com_vcp_id;
 //
 #define PIOS_PERIPHERAL_APB2_CLOCK					PIOS_SYSCLK
 
-
 //-------------------------
 // Interrupt Priorities
 //-------------------------
@@ -181,7 +182,6 @@ extern uint32_t pios_com_vcp_id;
 #define PIOS_IRQ_PRIO_MID                       8               // higher than RTOS
 #define PIOS_IRQ_PRIO_HIGH                      5               // for SPI, ADC, I2C etc...
 #define PIOS_IRQ_PRIO_HIGHEST                   4               // for USART etc...
-
 //------------------------
 // PIOS_RCVR
 // See also pios_board.c
@@ -235,8 +235,9 @@ extern uint32_t pios_com_vcp_id;
 // ADC
 // PIOS_ADC_PinGet(0) = Current sensor
 // PIOS_ADC_PinGet(1) = Voltage sensor
-// PIOS_ADC_PinGet(4) = VREF
+// PIOS_ADC_PinGet(2) = VREF
-// PIOS_ADC_PinGet(5) = Temperature sensor
+// PIOS_ADC_PinGet(3) = Temperature sensor
+// PIOS_ADC_PinGet(4) = Board Power
 //-------------------------
 #define PIOS_DMA_PIN_CONFIG                                                                         \
 {                                                                                                   \
@@ -253,7 +254,8 @@ extern uint32_t pios_com_vcp_id;
 #define PIOS_ADC_NUM_CHANNELS           4
 #define PIOS_ADC_MAX_OVERSAMPLING       2
 #define PIOS_ADC_USE_ADC2               0
-#define PIOS_ADC_VOLTAGE_SCALE 3.30/4096.0
+#define PIOS_ADC_VOLTAGE_SCALE 3.30f/4096.0f
+#define PIOS_ADC_USE_TEMP_SENSOR		1
 
 //-------------------------
 // USB

flight/targets/boards/simposix/firmware/UAVObjects.inc

@@ -83,6 +83,8 @@ UAVOBJSRCFILENAMES += velocitydesired
 UAVOBJSRCFILENAMES += watchdogstatus
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
+UAVOBJSRCFILENAMES += poilocation
+UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += flightstatus
 UAVOBJSRCFILENAMES += hwsettings
 UAVOBJSRCFILENAMES += receiveractivity

flight/uavobjects/uavobjectmanager.c

@@ -182,7 +182,11 @@ static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
 static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
 			UAVObjEventCallback cb);
 
-#if defined(PIOS_INCLUDE_SDCARD)
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD) && defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
+#error Both PIOS_USE_SETTINGS_ON_SDCARD and PIOS_INCLUDE_FLASH_SECTOR_SETTINGS. Only one settings storage allowed.
+#endif
+
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
 static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename);
 static void customSPrintf(uint8_t * buffer, uint8_t * format, ...);
 #endif
@@ -697,7 +701,7 @@ unlock_exit:
 	return rc;
 }
 
-#if defined(PIOS_INCLUDE_SDCARD)
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
 /**
  * Save the data of the specified object instance to the file system (SD card).
  * The object will be appended and the file will not be closed.
@@ -772,7 +776,7 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId,
 	xSemaphoreGiveRecursive(mutex);
 	return 0;
 }
-#endif /* PIOS_INCLUDE_SDCARD */
+#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
 
 /**
  * Save the data of the specified object to the file system (SD card).
@@ -808,7 +812,7 @@ int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
 			return -1;
 	}
 #endif
-#if defined(PIOS_INCLUDE_SDCARD)
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
 	FILEINFO file;
 	uint8_t filename[14];
 
@@ -836,11 +840,11 @@ int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
 	// Done, close file and unlock
 	PIOS_FCLOSE(file);
 	xSemaphoreGiveRecursive(mutex);
-#endif /* PIOS_INCLUDE_SDCARD */
+#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
 	return 0;
 }
 
-#if defined(PIOS_INCLUDE_SDCARD)
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
 /**
  * Load an object from the file system (SD card).
  * @param[in] file File to read from
@@ -928,7 +932,7 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
 	xSemaphoreGiveRecursive(mutex);
 	return obj_handle;
 }
-#endif /* PIOS_INCLUDE_SDCARD */
+#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
 
 /**
  * Load an object from the file system (SD card).
@@ -968,7 +972,7 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
 
 #endif
 
-#if defined(PIOS_INCLUDE_SDCARD)
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
 	FILEINFO file;
 	UAVObjHandle loadedObj;
 	uint8_t filename[14];
@@ -1004,7 +1008,7 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
 	// Done, close file and unlock
 	PIOS_FCLOSE(file);
 	xSemaphoreGiveRecursive(mutex);
-#endif /* PIOS_INCLUDE_SDCARD */
+#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
 	return 0;
 }
 
@@ -1020,7 +1024,7 @@ int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
 #if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
 	PIOS_FLASHFS_ObjDelete(0, UAVObjGetID(obj_handle), instId);
 #endif
-#if defined(PIOS_INCLUDE_SDCARD)
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
 	uint8_t filename[14];
 
 	// Check for file system availability
@@ -1038,7 +1042,7 @@ int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
 
 	// Done
 	xSemaphoreGiveRecursive(mutex);
-#endif /* PIOS_INCLUDE_SDCARD */
+#endif /* PIOS_USE_SETTINGS_ON_SDCARD */
 	return 0;
 }
 
@@ -2012,7 +2016,7 @@ static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
 	return -1;
 }
 
-#if defined(PIOS_INCLUDE_SDCARD)
+#if defined(PIOS_USE_SETTINGS_ON_SDCARD)
 /**
  * Wrapper for the sprintf function
  */
@@ -2030,4 +2034,4 @@ static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename)
 {
 	customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle));
 }
-#endif /* PIOS_INCLUDE_SDCARD */
+#endif /* PIOS_USE_SETTINGS_ON_SDCARD */

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp

@@ -28,7 +28,7 @@
 #include "gpsitem.h"
 namespace mapcontrol
 {
-    GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(5),traildistance(50),autosetreached(true)
+    GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(1),traildistance(2),autosetreached(true)
     ,autosetdistance(100)
     {
         pic.load(uavPic);
@@ -42,8 +42,10 @@ namespace mapcontrol
         trailLine=new QGraphicsItemGroup(this);
         trailLine->setParentItem(map);
         this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true);
+        setCacheMode(QGraphicsItem::ItemCoordinateCache);
         mapfollowtype=UAVMapFollowType::None;
-        trailtype=UAVTrailType::ByDistance;
+        //trailtype=UAVTrailType::ByDistance;
+        trailtype=UAVTrailType::ByTimeElapsed;
         timer.start();
         connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos()));
         connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal)));
@@ -74,12 +76,12 @@ namespace mapcontrol
             {
                 if(timer.elapsed()>trailtime*1000)
                 {
-                    TrailItem * ob=new TrailItem(position,altitude,Qt::green,map);
+                    TrailItem * ob=new TrailItem(position,altitude,Qt::red,map);
                     trail->addToGroup(ob);
                     connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT()));
                     if(!lasttrailline.IsEmpty())
                     {
-                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map);
+                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map);
                         trailLine->addToGroup(obj);
                         connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot()));
                     }
@@ -92,12 +94,12 @@ namespace mapcontrol
             {
                 if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance)
                 {
-                    TrailItem * ob=new TrailItem(position,altitude,Qt::green,map);
+                    TrailItem * ob=new TrailItem(position,altitude,Qt::red,map);
                     trail->addToGroup(ob);
                     connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT()));
                     if(!lasttrailline.IsEmpty())
                     {
-                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map);
+                        TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map);
                         trailLine->addToGroup(obj);
                         connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot()));
                     }

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp

@@ -80,6 +80,7 @@ namespace mapcontrol
 
             if(GPS!=0)
             {
+                GPS->DeleteTrail();
                 delete GPS;
                 GPS=0;
             }
@@ -93,6 +94,7 @@ namespace mapcontrol
             {
                 GPS=new GPSItem(map,this);
                 GPS->setParentItem(map);
+                GPS->setOpacity(overlayOpacity);
                 setOverlayOpacity(overlayOpacity);
             }
         }

ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/resources/cc_plugin.ini

@@ -25,7 +25,7 @@ cc_channel_6 = Ch13-FPV-Tilt
 ;cc_channel_10=
 
 ; Control TX or RX (before or after mixes)
-send_to = RX
+send_to = TX
 
 [Output]
 

ground/openpilotgcs/src/plugins/hitl/aerosimrc/src/udpconnect.cpp

@@ -87,7 +87,7 @@ void UdpSender::sendDatagram(const simToPlugin *stp)
     // attitude
     out << stp->heading     << stp->pitch       << stp->roll;
     // electric
-    out << stp->voltage     << stp->current;
+    out << stp->voltage     << stp->current     << stp->consumedCharge;
     // matrix
     out << stp->axisXx << stp->axisXy << stp->axisXz;
     out << stp->axisYx << stp->axisYy << stp->axisYz;

ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp

@@ -49,10 +49,11 @@ bool AeroSimRCSimulator::setupProcess()
 void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort)
 {
     Q_UNUSED(outPort)
-    if (inSocket->bind(QHostAddress(host), inPort))
+    if (inSocket->bind(QHostAddress(host), inPort)) {
         emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n");
-    else
+    } else {
         emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n");
+    }
 }
 
 void AeroSimRCSimulator::transmitUpdate()
@@ -71,13 +72,45 @@ void AeroSimRCSimulator::transmitUpdate()
         channels[i] = out;
     }
 
+    ActuatorDesired::DataFields actData;
+    FlightStatus::DataFields flightStatusData = flightStatus->getData();
+    ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
+    float roll = -1;
+    float pitch = -1;
+    float yaw = -1;
+    float throttle = -1;
+
+    if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
+        // Read joystick input
+        if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
+            // Note: Pitch sign is reversed in FG ?
+            roll = manCtrlData.Roll;
+            pitch = -manCtrlData.Pitch;
+            yaw = manCtrlData.Yaw;
+            throttle = manCtrlData.Throttle;
+        }
+    } else {
+        // Read ActuatorDesired from autopilot
+        actData = actDesired->getData();
+
+        roll = actData.Roll;
+        pitch = -actData.Pitch;
+        yaw = actData.Yaw;
+        throttle = (actData.Throttle*2.0)-1.0;
+    }
+    channels[0] = roll;
+    channels[1] = pitch;
+    if(throttle < -1) {
+        throttle = -1;
+    }
+    channels[2] = throttle;
+    channels[3] = yaw;
+
     // read flight status
-    FlightStatus::DataFields flightData;
-    flightData = flightStatus->getData();
     quint8 armed;
     quint8 mode;
-    armed = flightData.Armed;
+    armed = flightStatusData.Armed;
-    mode = flightData.FlightMode;
+    mode = flightStatusData.FlightMode;
 
     QByteArray data;
     // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32)
@@ -85,8 +118,9 @@ void AeroSimRCSimulator::transmitUpdate()
     QDataStream stream(&data, QIODevice::WriteOnly);
     stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
     stream << quint32(0x52434D44);      // magic header, "RCMD"
-    for (int i = 0; i < 10; ++i)
+    for (int i = 0; i < 10; ++i) {
         stream << channels[i];          // channels
+    }
     stream << armed << mode;            // flight status
     stream << udpCounterASrecv;         // packet counter
 
@@ -108,7 +142,7 @@ void AeroSimRCSimulator::transmitUpdate()
 void AeroSimRCSimulator::processUpdate(const QByteArray &data)
 {
     // check size
-    if (data.size() > 188) {
+    if (data.size() > 192) {
         qDebug() << "!!! big datagram: " << data.size();
         return;
     }
@@ -132,10 +166,11 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
             posX, posY, posZ,   // world
             velX, velY, velZ,   // world
             angX, angY, angZ,   // model
-            accX, accY, accZ,   // model
+            accX, accY, accZ;   // model
-            lat, lon, agl,      // world
+    qreal  lat, lon;
+    float   agl,      // world
             yaw, pitch, roll,   // model
-            volt, curr,
+            volt, curr, cons,
             rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix
             ch[AEROSIM_RCCHANNEL_NUMELEM];
 
@@ -148,7 +183,7 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
     stream >> accX >> accY >> accZ;
     stream >> lat >> lon >> agl;
     stream >> yaw >> pitch >> roll;
-    stream >> volt >> curr;
+    stream >> volt >> curr >> cons;
     stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
     stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7];
     stream >> udpCounterASrecv;
@@ -157,10 +192,10 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
     memset(&out, 0, sizeof(Output2Hardware));
 
 
-    out.delT=delT;
+    out.delT = delT;
 
     /*************************************************************************************/
-    for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++){
+    for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++) {
         out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1
     }
 
@@ -209,17 +244,20 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
     out.groundspeed = qSqrt(velX * velX + velY * velY);
 
     /**********************************************************************************************/
-    out.dstN = posY * 100;
+    out.dstN = posY;
-    out.dstE = posX * 100;
+    out.dstE = posX;
-    out.dstD = posZ * -100;
+    out.dstD = -posZ;
 
-    out.velDown = velY * 100;
+    out.velNorth = velY;
-    out.velEast = velX * 100;
+    out.velEast = velX;
-    out.velDown = velZ * 100; //WHY ISN'T THIS `-velZ`???
+    out.velDown = -velZ;
+
+    out.voltage = volt;
+    out.current = curr;
+    out.consumption = cons * 1000.0;
 
     updateUAVOs(out);
 
-
 #ifdef DBG_TIMERS
     static int cntRX = 0;
     if (cntRX >= 100) {

ground/openpilotgcs/src/plugins/hitl/hitloptionspage.ui

@@ -809,7 +809,7 @@
          <item>
           <widget class="QGroupBox" name="baroAltitudeCheckbox">
            <property name="enabled">
-            <bool>false</bool>
+            <bool>true</bool>
            </property>
            <property name="title">
             <string>BaroAltitude</string>

ground/openpilotgcs/src/plugins/hitl/simulator.cpp

@@ -69,6 +69,7 @@ Simulator::Simulator(const SimulatorSettings& params) :
     gcsRcvrTime = currentTime;
     attRawTime = currentTime;
     baroAltTime = currentTime;
+    battTime = currentTime;
     airspeedActualTime=currentTime;
 
     //Define standard atmospheric constants
@@ -153,6 +154,7 @@ void Simulator::onStart()
     velActual = VelocityActual::GetInstance(objManager);
     posActual = PositionActual::GetInstance(objManager);
     baroAlt = BaroAltitude::GetInstance(objManager);
+    flightBatt = FlightBatteryState::GetInstance(objManager);
     airspeedActual = AirspeedActual::GetInstance(objManager);
     attActual = AttitudeActual::GetInstance(objManager);
     attSettings = AttitudeSettings::GetInstance(objManager);
@@ -284,7 +286,10 @@ void Simulator::setupObjects()
         setupOutputObject(airspeedActual, settings.airspeedActualRate);
 
     if(settings.baroAltitudeEnabled)
+    {
         setupOutputObject(baroAlt, settings.baroAltRate);
+        setupOutputObject(flightBatt, settings.baroAltRate);
+    }
 
 }
 
@@ -726,6 +731,21 @@ void Simulator::updateUAVOs(Output2Hardware out){
         }
     }
 
+    /*******************************/
+    // Update FlightBatteryState object
+    if (settings.baroAltitudeEnabled){
+        if (battTime.msecsTo(currentTime) >= settings.baroAltRate) {
+        FlightBatteryState::DataFields batteryData;
+        memset(&batteryData, 0, sizeof(FlightBatteryState::DataFields));
+        batteryData.Voltage = out.voltage;
+        batteryData.Current = out.current;
+        batteryData.ConsumedEnergy = out.consumption;
+        flightBatt->setData(batteryData);
+
+        battTime=battTime.addMSecs(settings.baroAltRate);
+        }
+    }
+
     /*******************************/
     // Update AirspeedActual object
     if (settings.airspeedActualEnabled){

ground/openpilotgcs/src/plugins/hitl/simulator.h

@@ -45,6 +45,7 @@
 #include "attitudeactual.h"
 #include "attitudesettings.h"
 #include "baroaltitude.h"
+#include "flightbatterystate.h"
 #include "flightstatus.h"
 #include "gcsreceiver.h"
 #include "gcstelemetrystats.h"
@@ -194,6 +195,10 @@ struct Output2Hardware{
     float pitchDesired;
     float yawDesired;
     float throttleDesired;
+
+    float voltage;
+    float current;
+    float consumption;
 };
 
 //struct Output2Simulator{
@@ -285,6 +290,7 @@ protected:
     ActuatorDesired* actDesired;
     ManualControlCommand* manCtrlCommand;
     FlightStatus* flightStatus;
+    FlightBatteryState* flightBatt;
     BaroAltitude* baroAlt;
     AirspeedActual* airspeedActual;
     AttitudeActual* attActual;
@@ -323,6 +329,7 @@ private:
     QTime gpsPosTime;
     QTime groundTruthTime;
     QTime baroAltTime;
+    QTime battTime;
     QTime gcsRcvrTime;
     QTime airspeedActualTime;
 

ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp

@@ -200,6 +200,17 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
         ui->dsb_condParam4->setVisible(false);
         ui->condParam1->setText("Relative Distance(0=complete,1=just starting)");
         break;
+    case MapDataDelegate::ENDCONDITION_BELOWERROR:
+        ui->condParam1->setVisible(true);
+        ui->condParam2->setVisible(false);
+        ui->condParam3->setVisible(false);
+        ui->condParam4->setVisible(false);
+        ui->dsb_condParam1->setVisible(true);
+        ui->dsb_condParam2->setVisible(false);
+        ui->dsb_condParam3->setVisible(false);
+        ui->dsb_condParam4->setVisible(false);
+        ui->condParam1->setText("error margin (in m)");
+        break;
     case MapDataDelegate::ENDCONDITION_ABOVEALTITUDE:
         ui->condParam1->setVisible(true);
         ui->condParam2->setVisible(false);
@@ -211,6 +222,18 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
         ui->dsb_condParam4->setVisible(false);
         ui->condParam1->setText("Altitude in meters (negative)");
         break;
+    case MapDataDelegate::ENDCONDITION_ABOVESPEED:
+        ui->condParam1->setVisible(true);
+        ui->condParam2->setVisible(true);
+        ui->condParam3->setVisible(false);
+        ui->condParam4->setVisible(false);
+        ui->dsb_condParam1->setVisible(true);
+        ui->dsb_condParam2->setVisible(true);
+        ui->dsb_condParam3->setVisible(false);
+        ui->dsb_condParam4->setVisible(false);
+        ui->condParam1->setText("Speed in meters/second");
+        ui->condParam2->setText("flag: 0=groundspeed 1=airspeed");
+        break;
     case MapDataDelegate::ENDCONDITION_POINTINGTOWARDSNEXT:
         ui->condParam1->setVisible(true);
         ui->condParam2->setVisible(false);

ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp

@@ -24,6 +24,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#define USE_PATHPLANNER
 
 #include "opmapgadgetwidget.h"
 #include "ui_opmap_widget.h"
@@ -581,7 +582,7 @@ void OPMapGadgetWidget::updatePosition()
     gps_longitude = gpsPositionData.Longitude;
     gps_altitude = gpsPositionData.Altitude;
 
-	gps_pos = internals::PointLatLng(gps_latitude, gps_longitude);
+    gps_pos = internals::PointLatLng(gps_latitude*1e-7, gps_longitude*1e-7);
 
     //**********************
     // get the current position and heading estimates
@@ -643,6 +644,7 @@ void OPMapGadgetWidget::updatePosition()
     {
         m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position
         m_map->GPS->SetUAVHeading(gps_heading);       // set the maps GPS heading
+        m_map->GPS->update();
     }
     m_map->UAV->updateTextOverlay();
     m_map->UAV->update();

ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp

@@ -121,7 +121,9 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa
         combo->addItem("Timeout",ENDCONDITION_TIMEOUT);
         combo->addItem("Distance to tgt",ENDCONDITION_DISTANCETOTARGET);
         combo->addItem("Leg remaining",ENDCONDITION_LEGREMAINING);
+        combo->addItem("Below Error",ENDCONDITION_BELOWERROR);
         combo->addItem("Above Altitude",ENDCONDITION_ABOVEALTITUDE);
+        combo->addItem("Above Speed",ENDCONDITION_ABOVESPEED);
         combo->addItem("Pointing towards next",ENDCONDITION_POINTINGTOWARDSNEXT);
         combo->addItem("Python script",ENDCONDITION_PYTHONSCRIPT);
         combo->addItem("Immediate",ENDCONDITION_IMMEDIATE);

ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h

@@ -41,8 +41,9 @@ class MapDataDelegate : public QItemDelegate
                    MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, MODE_DRIVECIRCLERIGHT=7,
                    MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions;
     typedef enum { ENDCONDITION_NONE=0, ENDCONDITION_TIMEOUT=1, ENDCONDITION_DISTANCETOTARGET=2,
-                   ENDCONDITION_LEGREMAINING=3, ENDCONDITION_ABOVEALTITUDE=4, ENDCONDITION_POINTINGTOWARDSNEXT=5,
+                   ENDCONDITION_LEGREMAINING=3, ENDCONDITION_BELOWERROR=4, ENDCONDITION_ABOVEALTITUDE=5,
-                   ENDCONDITION_PYTHONSCRIPT=6, ENDCONDITION_IMMEDIATE=7 } EndConditionOptions;
+                   ENDCONDITION_ABOVESPEED=6, ENDCONDITION_POINTINGTOWARDSNEXT=7, ENDCONDITION_PYTHONSCRIPT=8,
+                   ENDCONDITION_IMMEDIATE=9 } EndConditionOptions;
     typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT=0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT=1,
                    COMMAND_ONCONDITIONJUMPWAYPOINT=2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT=3,
                    COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT=4 } CommandOptions;

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -95,6 +95,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/txpidsettings.h \
     $$UAVOBJECT_SYNTHETICS/cameradesired.h \
     $$UAVOBJECT_SYNTHETICS/faultsettings.h \
+    $$UAVOBJECT_SYNTHETICS/poilearnsettings.h \
+    $$UAVOBJECT_SYNTHETICS/poilocation.h \
     $$UAVOBJECT_SYNTHETICS/oplinksettings.h \
     $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \
     $$UAVOBJECT_SYNTHETICS/osdsettings.h \
@@ -175,6 +177,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/cameradesired.cpp \
     $$UAVOBJECT_SYNTHETICS/faultsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/poilearnsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/poilocation.cpp \
     $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \
     $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \

make/boot-defs.mk

@@ -112,9 +112,6 @@ BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE)
 BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION)
 BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE)
 BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION)
-BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
-BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
-BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
 
 # Compiler flags
 CDEFS += $(BLONLY_CDEFS)

make/common-defs.mk

@@ -95,6 +95,11 @@ endif
 #ADEFS = -DUSE_IRQ_ASM_WRAPPER
 ADEFS = -D__ASSEMBLY__
 
+# Provide board-specific defines
+CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE)
+CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE)
+CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE)
+
 # Compiler flag to set the C Standard level.
 # c89   - "ANSI" C
 # gnu89 - c89 plus GCC extensions

shared/uavobjectdefinition/flightstatus.xml

@@ -4,7 +4,7 @@
         <field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
 
 	<!-- Note these enumerated values should be the same as ManualControlSettings -->
-        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,PathPlanner"/>
+        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"/>
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/hwsettings.xml

@@ -2,19 +2,19 @@
 	<object name="HwSettings" singleinstance="true" settings="true">
 		<description>Selection of optional hardware configurations.</description>
 		<field name="CC_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,Outputs" defaultvalue="PWM"/>
-		<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Telemetry"/>
+		<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Telemetry"/>
-		<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,PPM,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Disabled"/>
+		<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,PPM,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
 
 		<field name="RV_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+Outputs,Outputs" defaultvalue="PWM"/>
-		<field name="RV_AuxPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge" defaultvalue="Disabled"/>
+		<field name="RV_AuxPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge,OsdHk" defaultvalue="Disabled"/>
-		<field name="RV_AuxSBusPort" units="function" type="enum" elements="1" options="Disabled,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge" defaultvalue="Disabled"/>
+		<field name="RV_AuxSBusPort" units="function" type="enum" elements="1" options="Disabled,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge,OsdHk" defaultvalue="Disabled"/>
 		<field name="RV_FlexiPort" units="function" type="enum" elements="1" options="Disabled,I2C,DSM2,DSMX (10bit),DSMX (11bit),ComAux,ComBridge" defaultvalue="Disabled"/>
 		<field name="RV_TelemetryPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,ComAux,ComBridge" defaultvalue="Telemetry"/>
 		<field name="RV_GPSPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,ComAux,ComBridge" defaultvalue="GPS"/>
 
 		<field name="RM_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Outputs,Outputs" defaultvalue="PWM"/>
-		<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Telemetry"/>
+		<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Telemetry"/>
-		<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge" defaultvalue="Disabled"/>
+		<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM2,DSMX (10bit),DSMX (11bit),DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
 
 		<field name="RadioPort" units="function" type="enum" elements="1" options="Disabled,Telemetry" defaultvalue="Disabled"/>
 		<field name="MaxRFPower" units="mW" type="enum" elements="1" options="1.25,1.6,3.16,6.3,12.6,25,50,100" defaultvalue="100"/>
@@ -26,7 +26,7 @@
 		<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
 		<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
 
-		<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo" options="Disabled,Enabled" defaultvalue="Disabled"/>
+		<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo,MagBaro,OsdHk" options="Disabled,Enabled" defaultvalue="Disabled"/>
 		<field name="ADCRouting" units="" type="enum" elementnames="ADC0,ADC1,ADC2,ADC3" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/>
 		<field name="DSMxBind" units=""  type="uint8"  elements="1" defaultvalue="0"/>
 

shared/uavobjectdefinition/manualcontrolsettings.xml

@@ -43,32 +43,32 @@
                units=""
                type="enum"
                elements="6"
-               options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner"
+               options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,Land,PathPlanner,POI"
                defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,PositionHold"
                limits="\
-                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
+                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
+                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
+                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
+                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner;\
+                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI;\
                        \
-                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0401NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner,\
+                       %0402NE:Autotune:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI,\
-                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner"/>
+                       %0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
 
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <field name="FailsafeBehavior" units="" type="enum" elements="1" options="None" defaultvalue="None"/>

shared/uavobjectdefinition/osdsettings.xml

@@ -14,6 +14,9 @@
         <field name="Heading" units="" type="enum" elements="1" options="Disabled,Enabled" defaultvalue="Enabled"/>
         <field name="HeadingSetup" units="" type="int16" elements="2" elementnames="X,Y" defaultvalue="168,240"/>
 		<field name="Screen" units=""  type="uint8"  elements="1" defaultvalue="0"/>
+		<field name="White" units=""  type="uint8"  elements="1" defaultvalue="4"/>
+		<field name="Black" units=""  type="uint8"  elements="1" defaultvalue="1"/>
+		<field name="AltitudeSource" units="" type="enum" elements="1" options="GPS,Baro" defaultvalue="GPS"/>
 	<access gcs="readwrite" flight="readwrite"/>
 	<telemetrygcs acked="true" updatemode="onchange" period="0"/>
 	<telemetryflight acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/poilearnsettings.xml

@@ -0,0 +1,10 @@
+<xml>
+    <object name="PoiLearnSettings" singleinstance="true" settings="true">
+        <description>Settings for the @ref Point of Interest feature</description>
+        <field name="Input" units="channel" type="enum" elements="1" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,None" defaultvalue="None"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="manual" period="0"/>
+	</object>
+</xml>

shared/uavobjectdefinition/poilocation.xml

@@ -0,0 +1,12 @@
+<xml>
+    <object name="PoiLocation" singleinstance="true" settings="false">
+        <description>Contains the current Point of interest relative to @ref HomeLocation</description>
+        <field name="North" units="m" type="float" elements="1"/>
+        <field name="East" units="m" type="float" elements="1"/>
+        <field name="Down" units="m" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="periodic" period="1000"/>
+    </object>
+</xml>

shared/uavobjectdefinition/stabilizationsettings.xml

@@ -5,7 +5,7 @@
 	<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="40" limits="%BE:0:180"/>
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="40" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
-	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>

shared/uavobjectdefinition/taskinfo.xml

@@ -27,6 +27,8 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>MagBaro</elementname>
+			<elementname>OSDGen</elementname>
 			<elementname>CallbackScheduler0</elementname>
 			<elementname>CallbackScheduler1</elementname>
 			<elementname>CallbackScheduler2</elementname>
@@ -59,6 +61,8 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>MagBaro</elementname>
+			<elementname>OSDGen</elementname>
 			<elementname>CallbackScheduler0</elementname>
 			<elementname>CallbackScheduler1</elementname>
 			<elementname>CallbackScheduler2</elementname>
@@ -95,6 +99,8 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>MagBaro</elementname>
+			<elementname>OSDGen</elementname>
 			<elementname>CallbackScheduler0</elementname>
 			<elementname>CallbackScheduler1</elementname>
 			<elementname>CallbackScheduler2</elementname>