Added UAVObjects: FlightSituationActual NavigationSettings NavigationDesired

- Added FlighSituation Module (development module for sensor fusion, mostly stub, possibly renamed later) - Added Navigation Module (development module for navigating towards a point in space - DEVELOPMENT CODE, NOT STABLE YET (I am testing around with this)) - Changed Stabilization Module (uses local reference frame now. Stable except for code cleanup/review. Tested in simulator and outperforms old code.) git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1154 ebee16cc-31ac-478f-84a7-5cbb03baadba
2025-02-20 10:54:14 +01:00 · 2010-07-26 21:26:28 +00:00 · 2010-07-26 21:26:28 +00:00 · 2621d07574
commit 2621d07574
parent 63ed78704c
31 changed files with 2665 additions and 27 deletions
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@ -163,6 +163,9 @@ SRC += $(OPUAVOBJ)/ahrsstatus.c
 SRC += $(OPUAVOBJ)/altitudeactual.c
 SRC += $(OPUAVOBJ)/attitudeactual.c
 SRC += $(OPUAVOBJ)/attitudesettings.c
+SRC += $(OPUAVOBJ)/flightsituationactual.c
+SRC += $(OPUAVOBJ)/navigationsettings.c
+SRC += $(OPUAVOBJ)/navigationdesired.c
 SRC += $(OPUAVOBJ)/flightbatterystate.c
 SRC += $(OPUAVOBJ)/headingactual.c
 endif
--- a/flight/OpenPilot/Makefile.posix
+++ b/flight/OpenPilot/Makefile.posix
@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD
 USE_THUMB_MODE = YES

 # List of modules to include
-MODULES =  Telemetry Stabilization
+MODULES =  Telemetry Stabilization FlightSituation Navigation
 #MODULES =  Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization
 #MODULES = Telemetry Example
 #MODULES = Telemetry MK/MKSerial
@ -149,6 +149,9 @@ SRC += $(OPUAVOBJ)/ahrsstatus.c
 SRC += $(OPUAVOBJ)/altitudeactual.c
 SRC += $(OPUAVOBJ)/attitudeactual.c
 SRC += $(OPUAVOBJ)/attitudesettings.c
+SRC += $(OPUAVOBJ)/flightsituationactual.c
+SRC += $(OPUAVOBJ)/navigationsettings.c
+SRC += $(OPUAVOBJ)/navigationdesired.c
 SRC += $(OPUAVOBJ)/flightbatterystate.c
 SRC += $(OPUAVOBJ)/headingactual.c
 endif
--- a/flight/OpenPilot/Modules/FlightSituation/flightsituation.c
+++ b/flight/OpenPilot/Modules/FlightSituation/flightsituation.c
@ -0,0 +1,160 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup FlightSituationModule FlightSituation Module
+ * @brief Sensor Merge of all Flight data into most likely situation
+ * @note This object updates the @ref FlightSituationActual UAVObject
+ * @{ 
+ *
+ * @file       flightsituation.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Attitude stabilization 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"
+#include "flightsituation.h"
+#include "flightsituationactual.h"
+#include "attitudeactual.h"
+#include "altitudeactual.h"
+#include "headingactual.h"
+#include "positionactual.h"
+#include "systemsettings.h"
+
+
+// Private constants
+#define STACK_SIZE configMINIMAL_STACK_SIZE
+#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
+
+// Private types
+
+// Private variables
+static xTaskHandle taskHandle;
+
+// Private functions
+static void flightSituationTask(void* parameters);
+
+/**
+ * Module initialization
+ */
+int32_t FlightSituationInitialize()
+{
+	// Initialize variables
+
+	// Start main task
+	xTaskCreate(flightSituationTask, (signed char*)"FlightSituation", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
+
+	return 0;
+}
+
+/**
+ * Module task
+ */
+static void flightSituationTask(void* parameters)
+{
+	AttitudeActualData attitudeActual;
+	AltitudeActualData altitudeActual;
+	HeadingActualData headingActual;
+	PositionActualData positionActual;
+	SystemSettingsData systemSettings;
+	FlightSituationActualData flightSituationActual;
+	portTickType lastSysTime;
+
+	// private variables
+	float altitudeLast=0.0;
+	// Main task loop
+	lastSysTime = xTaskGetTickCount();
+	while (1)
+	{
+		// Read settings and other objects
+		SystemSettingsGet(&systemSettings);
+		AttitudeActualGet(&attitudeActual);
+		AltitudeActualGet(&altitudeActual);
+		HeadingActualGet(&headingActual);
+		PositionActualGet(&positionActual);
+		FlightSituationActualGet(&flightSituationActual);
+
+		// TODO: plausibility check of GPS data,
+		// innertial navigation with kalman feed-in of GPS data
+		// sensor fusion
+		// STUB code:
+		flightSituationActual.Latitude = positionActual.Latitude;
+		flightSituationActual.Longitude = positionActual.Longitude;
+
+		
+		// TODO: fuse altitude information with GPS data plus 
+		// plausibility check
+		// STUB Code:
+		flightSituationActual.Altitude = altitudeActual.Altitude;
+
+		// TODO: get altitude over ground from somewhere:
+		// method 1: reflection sensor
+		// method 2: crude database with ground hight information
+		// method 3: manual setting of ground hight at start pos
+		// STUB code:
+		flightSituationActual.ATG = altitudeActual.Altitude;
+		
+		// TODO: use some more sophisticated Kalman filtering
+		// and several sources (including speed and pitch!)
+		// to get this one right
+		flightSituationActual.Climbrate = 0.9*flightSituationActual.Climbrate + 0.1*((flightSituationActual.Altitude-altitudeLast)*10);
+		// the times 10 is because timescale is 1/10th
+		// of a second right now
+		altitudeLast = flightSituationActual.Altitude;
+
+		// TODO: heading: sensor fusion from AttitudeActual.yaw
+		// and HeadingActual - with plausibility checks
+		// BUT ??? what to do with heli- / multicopters
+		// that can fly sideways?
+		// Maybe the AHRS can give us a movement vector too?
+		flightSituationActual.Heading = positionActual.Heading;
+
+		// TODO: airspeed - is THE critical measure to prevent stall
+		// and judge which maneuvers are safe.
+		// However AFAIK we have no sensor for it yet, do we?
+		// Even with moderate winds, a glider or other UAV
+		// can easily fly (seemingly) backwards, so airspeed
+		// and groundspeed can significantly differ!
+		flightSituationActual.Airspeed = positionActual.Groundspeed;
+
+		// TODO: this can possibly be taken from GPS
+		// with just a bit of plausibility checking
+		// and replacing by above if missing
+		flightSituationActual.Course = positionActual.Heading;
+		flightSituationActual.Groundspeed = positionActual.Groundspeed;
+
+
+		FlightSituationActualSet(&flightSituationActual);
+
+		// Clear alarms
+		// TODO create a new alarm
+		//AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
+
+		// Wait until next update
+		// TODO non-hardcoded update rate
+		vTaskDelayUntil(&lastSysTime, 100 / portTICK_RATE_MS );
+	}
+}
+
+/** 
+  * @}
+  * @}
+  */
--- a/flight/OpenPilot/Modules/FlightSituation/inc/flightsituation.h
+++ b/flight/OpenPilot/Modules/FlightSituation/inc/flightsituation.h
@ -0,0 +1,42 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup FlightSituationModule FlightSituation Module
+ * @brief Sensor Merge of all Flight data into most likely situation
+ * @note This object updates the @ref FlightSituationActual UAVObject
+ * @{ 
+ *
+ * @file       flightsituation.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Attitude stabilization 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 FLIGHTSITUATION_H
+#define FLIGHTSITUATION_H
+
+int32_t FlightSituationInitialize();
+
+#endif // FLIGHTSITUATION_H
+
+/**
+  * @}
+  * @}
+  */
--- a/flight/OpenPilot/Modules/Navigation/inc/navigation.h
+++ b/flight/OpenPilot/Modules/Navigation/inc/navigation.h
@ -0,0 +1,44 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup NavigationModule Navigation Module
+ * @brief Feeds Stabilization with input to fly to a given coordinate in space
+ * @note This object updates the @ref AttitudeDesired "Attitude Desired" based on the 
+ * comparison on the @ref NavigationDesired "Navigation Desired", @ref AttitudeActual
+ * "Attitude Actual" and @ref FlightSituationActual "FlightSituation Actual"
+ * @{ 
+ *
+ * @file       navigation.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Position stabilization 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 NAVIGATION_H
+#define NAVIGATION_H
+
+int32_t NavigationInitialize();
+
+#endif // NAVIGATION_H
+
+/**
+  * @}
+  * @}
+  */
--- a/flight/OpenPilot/Modules/Navigation/navigation.c
+++ b/flight/OpenPilot/Modules/Navigation/navigation.c
@ -0,0 +1,530 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup NavigationModule Navigation Module
+ * @brief Feeds Stabilization with input to fly to a given coordinate in space
+ * @note This object updates the @ref AttitudeDesired "Attitude Desired" based on the 
+ * comparison on the @ref NavigationDesired "Navigation Desired", @ref AttitudeActual
+ * "Attitude Actual" and @ref FlightSituationActual "FlightSituation Actual"
+ * @{ 
+ *
+ * @file       navigation.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Position stabilization 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"
+#include "navigation.h"
+#include "navigationsettings.h"
+#include "navigationdesired.h"
+#include "attitudeactual.h"
+#include "attitudedesired.h"
+#include "flightsituationactual.h"
+#include "manualcontrolcommand.h"
+#include "systemsettings.h"
+
+
+// Private constants
+#define STACK_SIZE configMINIMAL_STACK_SIZE
+#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
+#define DEG2RAD ( M_PI / 180.0 )
+#define RAD2DEG ( 180.0 / M_PI )
+#define MIN(a,b) ((a<b)?a:b)
+#define MAX(a,b) ((a>b)?a:b)
+#define SSQRT(x) ((x)>=0?sqrt(x):-sqrt(-x)) // signed square root
+#define EARTHRAD 6378137
+#define GRAVITY 9.81
+
+// Private types
+
+// Private variables
+static xTaskHandle taskHandle;
+
+// Private functions
+static void navigationTask(void* parameters);
+static float bound(float val, float min, float max);
+static float angleDifference(float val);
+static float fixAngle(float val,float min, float max);
+static float sphereDistance(float lat1,float long1,float lat2,float long2);
+static float sphereCourse(float lat1,float long1,float lat2,float long2);
+
+/**
+ * Module initialization
+ */
+int32_t NavigationInitialize()
+{
+	// Initialize variables
+
+	// Start main task
+	xTaskCreate(navigationTask, (signed char*)"Navigation", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
+
+	return 0;
+}
+
+/**
+ * Module task
+ */
+static void navigationTask(void* parameters)
+{
+	NavigationSettingsData navSettings;
+	NavigationDesiredData navigationDesired;
+	AttitudeActualData attitudeActual;
+	AttitudeDesiredData attitudeDesired;
+	FlightSituationActualData situationActual;
+	ManualControlCommandData manualControl;
+	SystemSettingsData systemSettings;
+	portTickType lastSysTime;
+
+	// flight safety values
+	float maxPitch;
+	float minPitch;
+	float maxAccel;
+
+	// helper variables
+	float safeAccel;
+	float safeDistance;
+	float aoa,saoa;
+
+	// intended flight direction
+	float targetDistance;
+	float targetVspeed;
+	float targetTruePitch;
+	float targetHeading;
+	float targetPitch;
+	float targetYaw;
+
+	// turn direction
+	float turnDirection;
+	float turnSpeed;
+	float turnAccel;
+
+
+	// Main task loop
+	lastSysTime = xTaskGetTickCount();
+	while (1)
+	{
+		// Read settings and other objects
+		NavigationSettingsGet(&navSettings);
+		SystemSettingsGet(&systemSettings);
+		ManualControlCommandGet(&manualControl);
+		FlightSituationActualGet(&situationActual);
+		NavigationDesiredGet(&navigationDesired);
+		AttitudeActualGet(&attitudeActual);
+
+		// horizontal distance to waypoint
+		// (not 100% exact since the earth is not a perfect sphere, but
+		// close enough for our purpose...)
+		targetDistance = DEG2RAD * EARTHRAD * sphereDistance(
+				situationActual.Latitude,situationActual.Longitude,
+				navigationDesired.Latitude,navigationDesired.Longitude
+			);
+
+		// pitch to climb/sink to target altitude (attempts to reach
+		// target altitude withing SettleTime seconds
+		targetVspeed= (navigationDesired.Altitude - situationActual.Altitude)/navSettings.SettleTime;
+		if ( targetVspeed >= situationActual.Airspeed )
+		{
+			targetTruePitch = 90.0;
+		}
+		else if ( targetVspeed <= -situationActual.Airspeed )
+		{
+			targetTruePitch = -90.0;
+		}
+		else
+		{
+			targetTruePitch = RAD2DEG * asin( targetVspeed / situationActual.Airspeed );
+		}
+
+		// course to target coordinate
+		targetHeading = sphereCourse(
+				situationActual.Latitude,situationActual.Longitude,
+				navigationDesired.Latitude,navigationDesired.Longitude
+			);
+
+//printf("\n\n\nTarget: Heading: %2f Distance: %2f climbing %2f°\n",targetHeading,targetDistance,targetTruePitch);
+//printf("distance: %f\n",sphereDistance(situationActual.Latitude,situationActual.Longitude,navigationDesired.Latitude,navigationDesired.Longitude));
+		/**
+		 * navigation for fixed wing planes
+		 */
+		if (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL)
+		{
+
+			/**
+			 * Fixed wing planes create lift through their airspeed
+			 * therefore most maneuver limits depend on the current
+			 * speed and situation.
+			 */
+
+			// acceleration:
+			maxAccel=navSettings.AccelerationMax;
+			/**
+			 * Idea: at minSpeed, the plane can JUST compensate 1g
+			 * safely.  The StallSpeed at any load factor is
+			 * Vstall=Vs(0) / sqrt(load) so the maximum load for
+			 * the current speed is maxLoad = (Vs(0)/V)²
+			 */
+			maxAccel = MIN( maxAccel , 
+					GRAVITY * (situationActual.Airspeed*situationActual.Airspeed)
+					/ (navSettings.SpeedMin * navSettings.SpeedMin)		
+				);
+
+			// maximum pitch:
+			maxPitch=85.0;
+
+			/**
+			 * Idea: any climb decreases speed. Aside from drag
+			 * speed decrease is vertical speed times g (9.81m/s²)
+			 * make sure speed stays above min speed within
+			 * SettleTime
+			 */
+			safeAccel = ( navSettings.SpeedSafe-situationActual.Airspeed ) / navSettings.SettleTime;
+			if (safeAccel>=0)
+			{
+				// speed is below minimum speed. Do not allow any climbs!
+				maxPitch = MIN(maxPitch,0);
+			}
+			else if (safeAccel>-GRAVITY)
+			{
+				// speed is getting low. Limit maximum climb!
+				maxPitch = MIN(maxPitch,RAD2DEG*asin(-safeAccel/GRAVITY));
+			}
+
+			// minimum pitch:
+			minPitch=-85.0;
+
+			/**
+			 * Idea: any dive increases speed. Aside from drag
+			 * speed increase is vertical speed times g (9.81m/s²)
+			 * make sure speed stays below max speed within
+			 * SettleTime
+			 */
+			safeAccel = ( navSettings.SpeedMax-situationActual.Airspeed ) / navSettings.SettleTime;
+			if (safeAccel<=0)
+			{
+				// speed is above maximum speed. Do not allow any dives!
+				minPitch = MAX(minPitch,0);
+			}
+			else if (safeAccel<GRAVITY)
+			{
+				// speed is getting high. Limit maximum dive!
+				minPitch = MAX(minPitch,-1.*RAD2DEG*asin(safeAccel/GRAVITY));
+			}
+
+			/**
+			 * Idea: To change pitch, acceleration must be
+			 * applied.  A pull up at maximum allowed pitch has a certain radius.
+			 * If we are lower than that we are not allowed to 
+			 */
+			safeDistance = situationActual.Airspeed * situationActual.Airspeed / (maxAccel+GRAVITY);
+//printf("safe distance: %f\n",safeDistance);
+			if (safeDistance <= 0 || situationActual.Altitude<=navigationDesired.Altitude)
+			{
+				// altitude is below wanted altitude. Do not allow any dive at all.
+				minPitch = MAX(minPitch,0);
+			}
+			else if ( safeDistance >= situationActual.Altitude - navigationDesired.Altitude)
+			{
+				// altitude is close to target altitude, limit negative pitch
+				minPitch = MAX(minPitch,-1 * RAD2DEG * asin( (situationActual.Altitude - navigationDesired.Altitude) / safeDistance ));
+			}
+
+//printf("Safety: Pitch: %2f %2f Accel: %2fm/s²\n",minPitch,maxPitch,maxAccel);
+
+			/**
+			 * Base anted yaw and pitch on calculated angle of
+			 * attack and side slip effects
+			 *
+			 * Note: This can lead to an oszillation between a
+			 * flight with positive and negative angle of attack.
+			 * Without this correction however, the plane will
+			 * always "lag behind" it's desired altitude.
+			 */
+
+			// angle of attack in X and Y
+			aoa = attitudeActual.Pitch - (RAD2DEG * atan2( situationActual.Climbrate, situationActual.Groundspeed));
+			saoa = attitudeActual.Yaw-situationActual.Course;
+
+			targetPitch = bound(targetTruePitch+aoa,minPitch,maxPitch);
+			targetYaw = fixAngle(targetHeading+saoa,0.,360.);
+//printf("Target                 Pitch: %2f Yaw: %2f \n",targetPitch,targetYaw);
+//printf("Current Roll: %2f Pitch: %2f Yaw: %2f \n",attitudeActual.Roll,attitudeActual.Pitch,attitudeActual.Yaw);
+
+			/**
+			 * Now we have the current Pitch Yaw and Roll in
+			 * AttitudeActual and the wanted course in targetPitch
+			 * and targetYaw. We also have the safety limits. Now
+			 * make a smooth and save transition.
+			 * including all safety limits! Make a nice transition.
+			 */
+
+			// turn vector (Euler)
+			/**
+			 * The naive approach wants to fly along an Orthodrome - the quickest way to get from one angle to another
+			 */
+			turnDirection = sphereCourse(attitudeActual.Pitch,attitudeActual.Yaw,targetPitch,targetYaw);
+
+			/**
+			 * However if the target yaw is more than 90° away, this orthodrome would lead through a minimum or maximum point that
+			 * could be beyond safe min/max pitch values!
+			 */
+			if (fabs(angleDifference(attitudeActual.Yaw-targetYaw))>90)
+			{
+				if (fabs(turnDirection)<90)
+				{
+					/**
+					 * Positive turns (upward) can conflict with maxPitch
+					 */
+					if (RAD2DEG*acos(sin(fabs(DEG2RAD*turnDirection))*cos(DEG2RAD*attitudeActual.Pitch))>maxPitch)
+					{
+						/**
+						 * In this case the turn has to be adjusted to a circle that won't conflict with maxPitch
+						 */
+//printf("maximum is %2f which is higher than %2f\n",RAD2DEG*acos(sin(DEG2RAD*turnDirection)*cos(DEG2RAD*attitudeActual.Pitch)),maxPitch);
+						if (cos(DEG2RAD*attitudeActual.Pitch)>cos(DEG2RAD*maxPitch))
+						{
+							if (turnDirection>0)
+							{
+								turnDirection = RAD2DEG * asin( cos(DEG2RAD*maxPitch)/cos(DEG2RAD*attitudeActual.Pitch));
+							}
+							else
+							{
+								turnDirection = -RAD2DEG * asin( cos(DEG2RAD*maxPitch)/cos(DEG2RAD*attitudeActual.Pitch));
+							}
+						}
+						else
+						{
+							/**
+							 * something went wrong (attitude outside safe bounds?)- compensate safely
+							 */
+							turnDirection=(turnDirection>0?110:-110);
+						}
+//printf("limiting because of max!\n");
+					}
+				} 
+				else
+				{
+					/**
+					 * Downward turns can conflict with minPitch
+					 */
+					if (RAD2DEG*acos(sin(fabs(DEG2RAD*turnDirection))*cos(DEG2RAD*attitudeActual.Pitch))>-minPitch)
+					{
+//printf("minimum is %2f which is less than %2f\n",-1*RAD2DEG*acos(sin(fabs(DEG2RAD*turnDirection))*cos(DEG2RAD*attitudeActual.Pitch)),minPitch);
+						/**
+						 * In this case the turn has to be adjusted to a circle that won't conflict with minPitch
+						 */
+						if (cos(DEG2RAD*attitudeActual.Pitch)>cos(DEG2RAD*minPitch))
+						{
+							if (turnDirection>0) {
+								turnDirection = 180 - RAD2DEG * asin( cos(DEG2RAD*minPitch)/cos(DEG2RAD*attitudeActual.Pitch));
+							}
+							else
+							{
+								turnDirection = -180 + RAD2DEG * asin( cos(DEG2RAD*minPitch)/cos(DEG2RAD*attitudeActual.Pitch));
+							}
+						}
+						else
+						{
+							/**
+							 * something went wrong (attitude outside safe bounds?)- compensate safely
+							 */
+							turnDirection=(turnDirection>0?45:-45);
+						}
+//printf("limiting because of min!\n");
+					}
+				}
+			}
+
+			/**
+			 * Turn speed in °/s is the minimum of
+			 * 	TurnSpeedFactor * degrees to turn
+			 * and
+			 * 	the maximum angular velocity achievable with
+			 * 	less than maxAccel centripetal acceleration
+			 * 	(taking gravity effect into account)
+			 * but not less than 0
+			 * Formula: acceleration = (DEG2RAD*turnSpeed)
+			 *                         * airspeed
+			 */
+			turnSpeed = MAX(0,MIN(
+					navSettings.TurnSpeedFactor * sphereDistance(
+									attitudeActual.Pitch,attitudeActual.Yaw,
+									targetPitch,targetYaw)
+					,
+						RAD2DEG * (maxAccel-( MIN(0,cos(DEG2RAD*turnDirection))*GRAVITY )) / situationActual.Airspeed
+					));
+//printf("turn distance is %2f °\n", sphereDistance(attitudeActual.Pitch,attitudeActual.Yaw,targetPitch,targetYaw));
+//printf("safe speed is %2f °/s\n",RAD2DEG * (maxAccel-( MIN(0,cos(DEG2RAD*turnDirection))*GRAVITY )) / situationActual.Airspeed);
+//printf("turn speed is %2f °/s\n",turnSpeed);
+//printf("Airspeed is %2f °/s\n",situationActual.Airspeed);
+
+			/**
+			 * Compute wanted centripetal acceleration
+			 */
+			turnAccel = DEG2RAD* turnSpeed * situationActual.Airspeed;
+//printf("Turn with: %2f at %2fm/s²\n",turnDirection,turnAccel);
+
+
+			// Desired Attitude
+			/**
+			 * Now that we know which way the plane is supposed to
+			 * go and at what acceleration, calculate the required
+			 * roll angle to also compensate gravity during the
+			 * turn
+			 */
+			attitudeDesired.Roll = RAD2DEG*atan2( 
+							turnAccel * sin(DEG2RAD*turnDirection),
+							turnAccel * cos(DEG2RAD*turnDirection) + cos(DEG2RAD*attitudeActual.Pitch)*GRAVITY
+							);
+			/**
+			 * However this can also result in an upright flight
+			 * with a load of less than 1g
+			 * In this case, the effect on yaw is reversed.
+			 * (Imagine rolling 45° right, then push the stick
+			 * slightly. You will go left and down!)
+			 * In this case we reverse the roll to descent into the
+			 * right direction!
+			 */
+			if (fabs(angleDifference(attitudeDesired.Roll-turnDirection))>90) {
+				attitudeDesired.Roll=-attitudeDesired.Roll;
+			}
+//printf("FINALS: roll %2f ",attitudeDesired.Roll);
+
+			/**
+			 * The desired Yaw and Pitch values are tricky.
+			 * The current Stabilization module doesn't allow us to
+			 * specify an angular speed or even centripetal
+			 * acceleration instead we have to specify an
+			 * orientation.
+			 * According to tests, a formula of
+			 * 	sqrt(turnAccel) * StabilizationForceFactor
+			 * degrees lookahead gives reasonable results.
+			 */
+			attitudeDesired.Pitch = bound(
+				attitudeActual.Pitch +  navSettings.StabilizationForceFactor * SSQRT(turnAccel * cos(DEG2RAD*turnDirection)),
+				minPitch,
+				maxPitch);
+			attitudeDesired.Yaw = attitudeActual.Yaw + navSettings.StabilizationForceFactor * SSQRT(turnAccel * sin(DEG2RAD*turnDirection)) ;
+//printf("pitch %2f  yaw %2f\n",attitudeDesired.Pitch-attitudeActual.Pitch,attitudeDesired.Yaw-attitudeActual.Yaw);
+
+
+			/**
+			 * TODO: Make a real PID loop for the throttle.
+			 */
+			attitudeDesired.Throttle=0.8;
+			
+
+		}
+
+		// Write actuator desired (if not in manual mode)
+		if ( manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
+		{
+			AttitudeDesiredSet(&attitudeDesired);
+		}
+
+		// Wait until next update
+		vTaskDelayUntil(&lastSysTime, navSettings.UpdatePeriod / portTICK_RATE_MS );
+	}
+}
+
+/**
+ * Bound input value between limits
+ */
+static float bound(float val, float min, float max)
+{
+	if ( val < min )
+	{
+		val = min;
+	}
+	else if ( val > max )
+	{
+		val = max;
+	}
+	return val;
+}
+
+/**
+ * Fix result of angular differences
+ */
+static float angleDifference(float val)
+{
+	while ( val < -180.0 )
+	{
+		val += 360.0;
+	}
+	while ( val > 180.0 )
+	{
+		val -= 360.0;
+	}
+	return val;
+}
+
+/**
+ * Make sure an angular value stays between correct bounds
+ */
+static float fixAngle(float val,float min, float max)
+{
+	while ( val < min )
+	{
+		val += 360.0;
+	}
+	while ( val >= max )
+	{
+		val -= 360.0;
+	}
+	return val;
+}
+
+/**
+ * calculate spherical distance and course between two coordinate pairs
+ * see http://de.wikipedia.org/wiki/Orthodrome for details
+ */
+static float sphereDistance(float lat1, float long1, float lat2, float long2)
+{
+	float zeta=(RAD2DEG * acos (
+		sin(DEG2RAD*lat1) * sin(DEG2RAD*lat2)
+		+ cos(DEG2RAD*lat1) * cos(DEG2RAD*lat2) * cos(DEG2RAD*(long2-long1))
+	));
+	if (isnan(zeta)) {
+		zeta=0;
+	}
+	return zeta;
+
+}
+static float sphereCourse(float lat1, float long1, float lat2, float long2)
+{
+	float zeta = sphereDistance(lat1,long1,lat2,long2);
+	float angle = RAD2DEG * acos(
+			( sin(DEG2RAD*lat2) - sin(DEG2RAD*lat1) * cos(DEG2RAD*zeta) )
+			/ ( cos(DEG2RAD*lat1) * sin(DEG2RAD*zeta) )
+		);
+	if (isnan(angle)) angle=0;
+	if (angleDifference(long2-long1)>=0) {
+		return angle;
+	} else {
+		return -angle;
+	}
+}
+
+/** 
+  * @}
+  * @}
+  */
--- a/flight/OpenPilot/Modules/Stabilization/stabilization.c
+++ b/flight/OpenPilot/Modules/Stabilization/stabilization.c
@ -47,6 +47,7 @@
 #define PITCH_INTEGRAL_LIMIT 0.5
 #define ROLL_INTEGRAL_LIMIT 0.5
 #define YAW_INTEGRAL_LIMIT 0.5
+#define DEG2RAD ( M_PI / 180.0 )

 // Private types

@ -56,6 +57,7 @@ static xTaskHandle taskHandle;
 // Private functions
 static void stabilizationTask(void* parameters);
 static float bound(float val, float min, float max);
+static float angleDifference(float val);

 /**
 * Module initialization
@ -82,23 +84,25 @@ static void stabilizationTask(void* parameters)
 	ManualControlCommandData manualControl;
 	SystemSettingsData systemSettings;
 	portTickType lastSysTime;
+	float pitchErrorGlobal, pitchErrorLastGlobal;
+	float yawErrorGlobal, yawErrorLastGlobal;
 	float pitchError, pitchErrorLast;
-	float rollError, rollErrorLast;
 	float yawError, yawErrorLast;
+	float rollError, rollErrorLast;
 	float pitchDerivative;
-	float rollDerivative;
 	float yawDerivative;
+	float rollDerivative;
 	float pitchIntegral, pitchIntegralLimit;
-	float rollIntegral, rollIntegralLimit;
 	float yawIntegral, yawIntegralLimit;
+	float rollIntegral, rollIntegralLimit;

 	// Initialize
 	pitchIntegral = 0.0;
-	rollIntegral = 0.0;
 	yawIntegral = 0.0;
-	pitchErrorLast = 0.0;
+	rollIntegral = 0.0;
+	pitchErrorLastGlobal = 0.0;
+	yawErrorLastGlobal = 0.0;
 	rollErrorLast = 0.0;
-	yawErrorLast = 0.0;

 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
@ -111,17 +115,70 @@ static void stabilizationTask(void* parameters)
 		AttitudeDesiredGet(&attitudeDesired);
 		AttitudeActualGet(&attitudeActual);

-		// Pitch stabilization control loop
-		pitchError = bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch;
+		// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.
+
+		// global pitch error
+		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
+		{
+			pitchErrorGlobal = angleDifference(
+					bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
+				);
+		} 
+		else
+		{
+			// in AUTO mode, Stabilization is used to steer the plane freely,
+			// while Navigation dictates the flight path, including maneuvers outside stable limits.
+			pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
+		}
+
+		// global yaw error
+		if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL || manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
+		{
+			// VTOLS consider yaw. AUTO mode considers YAW, too.
+			yawErrorGlobal = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
+		} else {
+			// FIXED WING STABILIZATION however does not.
+			yawErrorGlobal = 0;
+		}
+	
+		// local pitch error
+		pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
+		// local roll error (no translation needed - always local)
+		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
+		{
+			rollError = angleDifference(
+					bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
+				);
+		}
+		else
+		{
+			// in AUTO mode, Stabilization is used to steer the plane freely,
+			// while Navigation dictates the flight path, including maneuvers outside stable limits.
+			rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
+		}
+		// local yaw error
+		yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
+		
+		// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts
+
+		// pitch last
+		pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;
+
+		// yaw last
+		yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
+		
+		// global last variables are no longer needed 
+		pitchErrorLastGlobal = pitchErrorGlobal;
+		yawErrorLastGlobal = yawErrorGlobal;
+		
+		// local Pitch stabilization control loop
 		pitchDerivative = pitchError - pitchErrorLast;
 		pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
 		pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
 		actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
 		actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
-		pitchErrorLast = pitchError;

-		// Roll stabilization control loop
-		rollError = bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll;
+		// local Roll stabilization control loop
 		rollDerivative = rollError - rollErrorLast;
 		rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
 		rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
@ -129,22 +186,22 @@ static void stabilizationTask(void* parameters)
 		actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
 		rollErrorLast = rollError;

-		// Yaw stabilization control loop (only enabled on VTOL airframes)
+
+		// local Yaw stabilization control loop (only enabled on VTOL airframes)
 		if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )
 		{
-			yawError = attitudeDesired.Yaw - attitudeActual.Yaw;
 			yawDerivative = yawError - yawErrorLast;
 			yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
 			yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
 			actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
 			actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
-			yawErrorLast = yawError;
 		}
 		else
 		{
 			actuatorDesired.Yaw = 0.0;
 		}

+
 		// Setup throttle
 		actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);

@ -156,11 +213,11 @@ static void stabilizationTask(void* parameters)
 		else
 		{
 			pitchIntegral = 0.0;
-			rollIntegral = 0.0;
 			yawIntegral = 0.0;
-			pitchErrorLast = 0.0;
+			rollIntegral = 0.0;
+			pitchErrorLastGlobal = 0.0;
+			yawErrorLastGlobal = 0.0;
 			rollErrorLast = 0.0;
-			yawErrorLast = 0.0;
 		}

 		// Clear alarms
@ -187,6 +244,22 @@ static float bound(float val, float min, float max)
 	return val;
 }

+/**
+ * Fix result of angular differences
+ */
+static float angleDifference(float val)
+{
+	while ( val < -180.0 )
+	{
+		val += 360.0;
+	}
+	while ( val > 180.0 )
+	{
+		val -= 360.0;
+	}
+	return val;
+}
+
 /** 
  * @}
  * @}
--- a/flight/OpenPilot/UAVObjects/flightsituationactual.c
+++ b/flight/OpenPilot/UAVObjects/flightsituationactual.c
@ -0,0 +1,102 @@
+/**
+ ******************************************************************************
+ *
+ * @file       flightsituationactual.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the FlightSituationActual object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: flightsituationactual.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "openpilot.h"
+#include "flightsituationactual.h"
+
+// Private variables
+static UAVObjHandle handle;
+
+// Private functions
+static void setDefaults(UAVObjHandle obj, uint16_t instId);
+
+/**
+ * Initialize object.
+ * \return 0 Success
+ * \return -1 Failure
+ */
+int32_t FlightSituationActualInitialize()
+{
+	// Register object with the object manager
+	handle = UAVObjRegister(FLIGHTSITUATIONACTUAL_OBJID, FLIGHTSITUATIONACTUAL_NAME, FLIGHTSITUATIONACTUAL_METANAME, 0,
+			FLIGHTSITUATIONACTUAL_ISSINGLEINST, FLIGHTSITUATIONACTUAL_ISSETTINGS, FLIGHTSITUATIONACTUAL_NUMBYTES, &setDefaults);
+
+	// Done
+	if (handle != 0)
+	{
+		return 0;
+	}
+	else
+	{
+		return -1;
+	}
+}
+
+/**
+ * Initialize object fields and metadata with the default values.
+ * If a default value is not specified the object fields
+ * will be initialized to zero.
+ */
+static void setDefaults(UAVObjHandle obj, uint16_t instId)
+{
+	FlightSituationActualData data;
+	UAVObjMetadata metadata;
+
+	// Initialize object fields to their default values
+	UAVObjGetInstanceData(obj, instId, &data);
+	memset(&data, 0, sizeof(FlightSituationActualData));
+
+	UAVObjSetInstanceData(obj, instId, &data);
+
+	// Initialize object metadata to their default values
+	metadata.access = ACCESS_READWRITE;
+	metadata.gcsAccess = ACCESS_READWRITE;
+	metadata.telemetryAcked = 0;
+	metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
+	metadata.telemetryUpdatePeriod = 1000;
+	metadata.gcsTelemetryAcked = 0;
+	metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
+	metadata.gcsTelemetryUpdatePeriod = 0;
+	metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
+	metadata.loggingUpdatePeriod = 1000;
+	UAVObjSetMetadata(obj, &metadata);
+}
+
+/**
+ * Get object handle
+ */
+UAVObjHandle FlightSituationActualHandle()
+{
+	return handle;
+}
+
+
+
--- a/flight/OpenPilot/UAVObjects/inc/flightsituationactual.h
+++ b/flight/OpenPilot/UAVObjects/inc/flightsituationactual.h
@ -0,0 +1,88 @@
+/**
+ ******************************************************************************
+ *
+ * @file       flightsituationactual.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the FlightSituationActual object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: flightsituationactual.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @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 FLIGHTSITUATIONACTUAL_H
+#define FLIGHTSITUATIONACTUAL_H
+
+// Object constants
+#define FLIGHTSITUATIONACTUAL_OBJID 499301246U
+#define FLIGHTSITUATIONACTUAL_NAME "FlightSituationActual"
+#define FLIGHTSITUATIONACTUAL_METANAME "FlightSituationActualMeta"
+#define FLIGHTSITUATIONACTUAL_ISSINGLEINST 1
+#define FLIGHTSITUATIONACTUAL_ISSETTINGS 0
+#define FLIGHTSITUATIONACTUAL_NUMBYTES sizeof(FlightSituationActualData)
+
+// Object access macros
+#define FlightSituationActualGet(dataOut) UAVObjGetData(FlightSituationActualHandle(), dataOut)
+#define FlightSituationActualSet(dataIn) UAVObjSetData(FlightSituationActualHandle(), dataIn)
+#define FlightSituationActualInstGet(instId, dataOut) UAVObjGetInstanceData(FlightSituationActualHandle(), instId, dataOut)
+#define FlightSituationActualInstSet(instId, dataIn) UAVObjSetInstanceData(FlightSituationActualHandle(), instId, dataIn)
+#define FlightSituationActualConnectQueue(queue) UAVObjConnectQueue(FlightSituationActualHandle(), queue, EV_MASK_ALL_UPDATES)
+#define FlightSituationActualConnectCallback(cb) UAVObjConnectCallback(FlightSituationActualHandle(), cb, EV_MASK_ALL_UPDATES)
+#define FlightSituationActualCreateInstance() UAVObjCreateInstance(FlightSituationActualHandle())
+#define FlightSituationActualRequestUpdate() UAVObjRequestUpdate(FlightSituationActualHandle())
+#define FlightSituationActualRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(FlightSituationActualHandle(), instId)
+#define FlightSituationActualUpdated() UAVObjUpdated(FlightSituationActualHandle())
+#define FlightSituationActualInstUpdated(instId) UAVObjUpdated(FlightSituationActualHandle(), instId)
+#define FlightSituationActualGetMetadata(dataOut) UAVObjGetMetadata(FlightSituationActualHandle(), dataOut)
+#define FlightSituationActualSetMetadata(dataIn) UAVObjSetMetadata(FlightSituationActualHandle(), dataIn)
+
+// Object data
+typedef struct {
+    float Latitude;
+    float Longitude;
+    float Altitude;
+    float ATG;
+    float Climbrate;
+    float Heading;
+    float Airspeed;
+    float Course;
+    float Groundspeed;
+
+} __attribute__((packed)) FlightSituationActualData;
+
+// Field information
+// Field Latitude information
+// Field Longitude information
+// Field Altitude information
+// Field ATG information
+// Field Climbrate information
+// Field Heading information
+// Field Airspeed information
+// Field Course information
+// Field Groundspeed information
+
+
+// Generic interface functions
+int32_t FlightSituationActualInitialize();
+UAVObjHandle FlightSituationActualHandle();
+
+#endif // FLIGHTSITUATIONACTUAL_H
--- a/flight/OpenPilot/UAVObjects/inc/navigationdesired.h
+++ b/flight/OpenPilot/UAVObjects/inc/navigationdesired.h
@ -0,0 +1,78 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationdesired.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the NavigationDesired object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: navigationdesired.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @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 NAVIGATIONDESIRED_H
+#define NAVIGATIONDESIRED_H
+
+// Object constants
+#define NAVIGATIONDESIRED_OBJID 2357718118U
+#define NAVIGATIONDESIRED_NAME "NavigationDesired"
+#define NAVIGATIONDESIRED_METANAME "NavigationDesiredMeta"
+#define NAVIGATIONDESIRED_ISSINGLEINST 1
+#define NAVIGATIONDESIRED_ISSETTINGS 0
+#define NAVIGATIONDESIRED_NUMBYTES sizeof(NavigationDesiredData)
+
+// Object access macros
+#define NavigationDesiredGet(dataOut) UAVObjGetData(NavigationDesiredHandle(), dataOut)
+#define NavigationDesiredSet(dataIn) UAVObjSetData(NavigationDesiredHandle(), dataIn)
+#define NavigationDesiredInstGet(instId, dataOut) UAVObjGetInstanceData(NavigationDesiredHandle(), instId, dataOut)
+#define NavigationDesiredInstSet(instId, dataIn) UAVObjSetInstanceData(NavigationDesiredHandle(), instId, dataIn)
+#define NavigationDesiredConnectQueue(queue) UAVObjConnectQueue(NavigationDesiredHandle(), queue, EV_MASK_ALL_UPDATES)
+#define NavigationDesiredConnectCallback(cb) UAVObjConnectCallback(NavigationDesiredHandle(), cb, EV_MASK_ALL_UPDATES)
+#define NavigationDesiredCreateInstance() UAVObjCreateInstance(NavigationDesiredHandle())
+#define NavigationDesiredRequestUpdate() UAVObjRequestUpdate(NavigationDesiredHandle())
+#define NavigationDesiredRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(NavigationDesiredHandle(), instId)
+#define NavigationDesiredUpdated() UAVObjUpdated(NavigationDesiredHandle())
+#define NavigationDesiredInstUpdated(instId) UAVObjUpdated(NavigationDesiredHandle(), instId)
+#define NavigationDesiredGetMetadata(dataOut) UAVObjGetMetadata(NavigationDesiredHandle(), dataOut)
+#define NavigationDesiredSetMetadata(dataIn) UAVObjSetMetadata(NavigationDesiredHandle(), dataIn)
+
+// Object data
+typedef struct {
+    float Latitude;
+    float Longitude;
+    float Altitude;
+    float Speed;
+
+} __attribute__((packed)) NavigationDesiredData;
+
+// Field information
+// Field Latitude information
+// Field Longitude information
+// Field Altitude information
+// Field Speed information
+
+
+// Generic interface functions
+int32_t NavigationDesiredInitialize();
+UAVObjHandle NavigationDesiredHandle();
+
+#endif // NAVIGATIONDESIRED_H
--- a/flight/OpenPilot/UAVObjects/inc/navigationsettings.h
+++ b/flight/OpenPilot/UAVObjects/inc/navigationsettings.h
@ -0,0 +1,86 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationsettings.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the NavigationSettings object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: navigationsettings.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @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 NAVIGATIONSETTINGS_H
+#define NAVIGATIONSETTINGS_H
+
+// Object constants
+#define NAVIGATIONSETTINGS_OBJID 1350892244U
+#define NAVIGATIONSETTINGS_NAME "NavigationSettings"
+#define NAVIGATIONSETTINGS_METANAME "NavigationSettingsMeta"
+#define NAVIGATIONSETTINGS_ISSINGLEINST 1
+#define NAVIGATIONSETTINGS_ISSETTINGS 1
+#define NAVIGATIONSETTINGS_NUMBYTES sizeof(NavigationSettingsData)
+
+// Object access macros
+#define NavigationSettingsGet(dataOut) UAVObjGetData(NavigationSettingsHandle(), dataOut)
+#define NavigationSettingsSet(dataIn) UAVObjSetData(NavigationSettingsHandle(), dataIn)
+#define NavigationSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(NavigationSettingsHandle(), instId, dataOut)
+#define NavigationSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(NavigationSettingsHandle(), instId, dataIn)
+#define NavigationSettingsConnectQueue(queue) UAVObjConnectQueue(NavigationSettingsHandle(), queue, EV_MASK_ALL_UPDATES)
+#define NavigationSettingsConnectCallback(cb) UAVObjConnectCallback(NavigationSettingsHandle(), cb, EV_MASK_ALL_UPDATES)
+#define NavigationSettingsCreateInstance() UAVObjCreateInstance(NavigationSettingsHandle())
+#define NavigationSettingsRequestUpdate() UAVObjRequestUpdate(NavigationSettingsHandle())
+#define NavigationSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(NavigationSettingsHandle(), instId)
+#define NavigationSettingsUpdated() UAVObjUpdated(NavigationSettingsHandle())
+#define NavigationSettingsInstUpdated(instId) UAVObjUpdated(NavigationSettingsHandle(), instId)
+#define NavigationSettingsGetMetadata(dataOut) UAVObjGetMetadata(NavigationSettingsHandle(), dataOut)
+#define NavigationSettingsSetMetadata(dataIn) UAVObjSetMetadata(NavigationSettingsHandle(), dataIn)
+
+// Object data
+typedef struct {
+    uint16_t UpdatePeriod;
+    float AccelerationMax;
+    float SpeedMax;
+    float SpeedSafe;
+    float SpeedMin;
+    float SettleTime;
+    float TurnSpeedFactor;
+    float StabilizationForceFactor;
+
+} __attribute__((packed)) NavigationSettingsData;
+
+// Field information
+// Field UpdatePeriod information
+// Field AccelerationMax information
+// Field SpeedMax information
+// Field SpeedSafe information
+// Field SpeedMin information
+// Field SettleTime information
+// Field TurnSpeedFactor information
+// Field StabilizationForceFactor information
+
+
+// Generic interface functions
+int32_t NavigationSettingsInitialize();
+UAVObjHandle NavigationSettingsHandle();
+
+#endif // NAVIGATIONSETTINGS_H
--- a/flight/OpenPilot/UAVObjects/navigationdesired.c
+++ b/flight/OpenPilot/UAVObjects/navigationdesired.c
@ -0,0 +1,102 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationdesired.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the NavigationDesired object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: navigationdesired.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "openpilot.h"
+#include "navigationdesired.h"
+
+// Private variables
+static UAVObjHandle handle;
+
+// Private functions
+static void setDefaults(UAVObjHandle obj, uint16_t instId);
+
+/**
+ * Initialize object.
+ * \return 0 Success
+ * \return -1 Failure
+ */
+int32_t NavigationDesiredInitialize()
+{
+	// Register object with the object manager
+	handle = UAVObjRegister(NAVIGATIONDESIRED_OBJID, NAVIGATIONDESIRED_NAME, NAVIGATIONDESIRED_METANAME, 0,
+			NAVIGATIONDESIRED_ISSINGLEINST, NAVIGATIONDESIRED_ISSETTINGS, NAVIGATIONDESIRED_NUMBYTES, &setDefaults);
+
+	// Done
+	if (handle != 0)
+	{
+		return 0;
+	}
+	else
+	{
+		return -1;
+	}
+}
+
+/**
+ * Initialize object fields and metadata with the default values.
+ * If a default value is not specified the object fields
+ * will be initialized to zero.
+ */
+static void setDefaults(UAVObjHandle obj, uint16_t instId)
+{
+	NavigationDesiredData data;
+	UAVObjMetadata metadata;
+
+	// Initialize object fields to their default values
+	UAVObjGetInstanceData(obj, instId, &data);
+	memset(&data, 0, sizeof(NavigationDesiredData));
+
+	UAVObjSetInstanceData(obj, instId, &data);
+
+	// Initialize object metadata to their default values
+	metadata.access = ACCESS_READWRITE;
+	metadata.gcsAccess = ACCESS_READWRITE;
+	metadata.telemetryAcked = 0;
+	metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
+	metadata.telemetryUpdatePeriod = 1000;
+	metadata.gcsTelemetryAcked = 0;
+	metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
+	metadata.gcsTelemetryUpdatePeriod = 0;
+	metadata.loggingUpdateMode = UPDATEMODE_NEVER;
+	metadata.loggingUpdatePeriod = 0;
+	UAVObjSetMetadata(obj, &metadata);
+}
+
+/**
+ * Get object handle
+ */
+UAVObjHandle NavigationDesiredHandle()
+{
+	return handle;
+}
+
+
+
--- a/flight/OpenPilot/UAVObjects/navigationsettings.c
+++ b/flight/OpenPilot/UAVObjects/navigationsettings.c
@ -0,0 +1,110 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationsettings.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the NavigationSettings object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: navigationsettings.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "openpilot.h"
+#include "navigationsettings.h"
+
+// Private variables
+static UAVObjHandle handle;
+
+// Private functions
+static void setDefaults(UAVObjHandle obj, uint16_t instId);
+
+/**
+ * Initialize object.
+ * \return 0 Success
+ * \return -1 Failure
+ */
+int32_t NavigationSettingsInitialize()
+{
+	// Register object with the object manager
+	handle = UAVObjRegister(NAVIGATIONSETTINGS_OBJID, NAVIGATIONSETTINGS_NAME, NAVIGATIONSETTINGS_METANAME, 0,
+			NAVIGATIONSETTINGS_ISSINGLEINST, NAVIGATIONSETTINGS_ISSETTINGS, NAVIGATIONSETTINGS_NUMBYTES, &setDefaults);
+
+	// Done
+	if (handle != 0)
+	{
+		return 0;
+	}
+	else
+	{
+		return -1;
+	}
+}
+
+/**
+ * Initialize object fields and metadata with the default values.
+ * If a default value is not specified the object fields
+ * will be initialized to zero.
+ */
+static void setDefaults(UAVObjHandle obj, uint16_t instId)
+{
+	NavigationSettingsData data;
+	UAVObjMetadata metadata;
+
+	// Initialize object fields to their default values
+	UAVObjGetInstanceData(obj, instId, &data);
+	memset(&data, 0, sizeof(NavigationSettingsData));
+    data.UpdatePeriod = 100;
+    data.AccelerationMax = 35;
+    data.SpeedMax = 300;
+    data.SpeedSafe = 100;
+    data.SpeedMin = 70;
+    data.SettleTime = 12;
+    data.TurnSpeedFactor = 0.1;
+    data.StabilizationForceFactor = 4;
+
+	UAVObjSetInstanceData(obj, instId, &data);
+
+	// Initialize object metadata to their default values
+	metadata.access = ACCESS_READWRITE;
+	metadata.gcsAccess = ACCESS_READWRITE;
+	metadata.telemetryAcked = 1;
+	metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
+	metadata.telemetryUpdatePeriod = 0;
+	metadata.gcsTelemetryAcked = 1;
+	metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
+	metadata.gcsTelemetryUpdatePeriod = 0;
+	metadata.loggingUpdateMode = UPDATEMODE_NEVER;
+	metadata.loggingUpdatePeriod = 0;
+	UAVObjSetMetadata(obj, &metadata);
+}
+
+/**
+ * Get object handle
+ */
+UAVObjHandle NavigationSettingsHandle()
+{
+	return handle;
+}
+
+
+
--- a/flight/OpenPilot/UAVObjects/stabilizationsettings.c
+++ b/flight/OpenPilot/UAVObjects/stabilizationsettings.c
@ -83,9 +83,9 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
    data.RollKp = 0.02;
    data.RollKi = 4e-06;
    data.RollKd = 0.01;
-    data.YawKp = 1;
-    data.YawKi = 0;
-    data.YawKd = 0;
+    data.YawKp = 0.04;
+    data.YawKi = 4e-06;
+    data.YawKd = 0.01;

 	UAVObjSetInstanceData(obj, instId, &data);

--- a/flight/OpenPilot/UAVObjects/uavobjectsinit.c
+++ b/flight/OpenPilot/UAVObjects/uavobjectsinit.c
@ -40,11 +40,14 @@
 #include "exampleobject2.h"
 #include "examplesettings.h"
 #include "flightbatterystate.h"
+#include "flightsituationactual.h"
 #include "flighttelemetrystats.h"
 #include "gcstelemetrystats.h"
 #include "headingactual.h"
 #include "manualcontrolcommand.h"
 #include "manualcontrolsettings.h"
+#include "navigationdesired.h"
+#include "navigationsettings.h"
 #include "objectpersistence.h"
 #include "positionactual.h"
 #include "stabilizationsettings.h"
@ -72,11 +75,14 @@ void UAVObjectsInitializeAll()
    ExampleObject2Initialize();
    ExampleSettingsInitialize();
    FlightBatteryStateInitialize();
+    FlightSituationActualInitialize();
    FlightTelemetryStatsInitialize();
    GCSTelemetryStatsInitialize();
    HeadingActualInitialize();
    ManualControlCommandInitialize();
    ManualControlSettingsInitialize();
+    NavigationDesiredInitialize();
+    NavigationSettingsInitialize();
    ObjectPersistenceInitialize();
    PositionActualInitialize();
    StabilizationSettingsInitialize();
--- a/ground/src/plugins/uavobjects/flightsituationactual.cpp
+++ b/ground/src/plugins/uavobjects/flightsituationactual.cpp
@ -0,0 +1,152 @@
+/**
+ ******************************************************************************
+ *
+ * @file       flightsituationactual.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectsPlugin UAVObjects Plugin
+ * @{
+ *   
+ * @note       Object definition file: flightsituationactual.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @brief      The UAVUObjects GCS plugin
+ *****************************************************************************/
+/* 
+ * 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 "flightsituationactual.h"
+#include "uavobjectfield.h"
+
+const QString FlightSituationActual::NAME = QString("FlightSituationActual");
+
+/**
+ * Constructor
+ */
+FlightSituationActual::FlightSituationActual(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
+{
+    // Create fields
+    QList<UAVObjectField*> fields;
+    QStringList LatitudeElemNames;
+    LatitudeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Latitude"), QString("degrees"), UAVObjectField::FLOAT32, LatitudeElemNames, QStringList()) );
+    QStringList LongitudeElemNames;
+    LongitudeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Longitude"), QString("degrees"), UAVObjectField::FLOAT32, LongitudeElemNames, QStringList()) );
+    QStringList AltitudeElemNames;
+    AltitudeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Altitude"), QString("meters"), UAVObjectField::FLOAT32, AltitudeElemNames, QStringList()) );
+    QStringList ATGElemNames;
+    ATGElemNames.append("0");
+    fields.append( new UAVObjectField(QString("ATG"), QString("meters"), UAVObjectField::FLOAT32, ATGElemNames, QStringList()) );
+    QStringList ClimbrateElemNames;
+    ClimbrateElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Climbrate"), QString("m/s"), UAVObjectField::FLOAT32, ClimbrateElemNames, QStringList()) );
+    QStringList HeadingElemNames;
+    HeadingElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Heading"), QString("degrees"), UAVObjectField::FLOAT32, HeadingElemNames, QStringList()) );
+    QStringList AirspeedElemNames;
+    AirspeedElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Airspeed"), QString("m/s"), UAVObjectField::FLOAT32, AirspeedElemNames, QStringList()) );
+    QStringList CourseElemNames;
+    CourseElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Course"), QString("degrees"), UAVObjectField::FLOAT32, CourseElemNames, QStringList()) );
+    QStringList GroundspeedElemNames;
+    GroundspeedElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Groundspeed"), QString("m/s"), UAVObjectField::FLOAT32, GroundspeedElemNames, QStringList()) );
+
+    // Initialize object
+    initializeFields(fields, (quint8*)&data, NUMBYTES);
+    // Set the default field values
+    setDefaultFieldValues();
+}
+
+/**
+ * Get the default metadata for this object
+ */
+UAVObject::Metadata FlightSituationActual::getDefaultMetadata()
+{
+    UAVObject::Metadata metadata;
+    metadata.flightAccess = ACCESS_READWRITE;
+    metadata.gcsAccess = ACCESS_READWRITE;
+    metadata.gcsTelemetryAcked = 0;
+    metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL;
+    metadata.gcsTelemetryUpdatePeriod = 0;
+    metadata.flightTelemetryAcked = 0;
+    metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
+    metadata.flightTelemetryUpdatePeriod = 1000;
+    metadata.loggingUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
+    metadata.loggingUpdatePeriod = 1000;
+    return metadata;
+}
+
+/**
+ * Initialize object fields with the default values.
+ * If a default value is not specified the object fields
+ * will be initialized to zero.
+ */
+void FlightSituationActual::setDefaultFieldValues()
+{
+
+}
+
+/**
+ * Get the object data fields
+ */
+FlightSituationActual::DataFields FlightSituationActual::getData()
+{
+    QMutexLocker locker(mutex);
+    return data;
+}
+
+/**
+ * Set the object data fields
+ */
+void FlightSituationActual::setData(const DataFields& data)
+{
+    QMutexLocker locker(mutex);
+    // Get metadata
+    Metadata mdata = getMetadata();
+    // Update object if the access mode permits
+    if ( mdata.gcsAccess == ACCESS_READWRITE )
+    {
+        this->data = data;
+        emit objectUpdatedAuto(this); // trigger object updated event
+        emit objectUpdated(this);
+    }
+}
+
+/**
+ * Create a clone of this object, a new instance ID must be specified.
+ * Do not use this function directly to create new instances, the
+ * UAVObjectManager should be used instead.
+ */
+UAVDataObject* FlightSituationActual::clone(quint32 instID)
+{
+    FlightSituationActual* obj = new FlightSituationActual();
+    obj->initialize(instID, this->getMetaObject());
+    return obj;
+}
+
+/**
+ * Static function to retrieve an instance of the object.
+ */
+FlightSituationActual* FlightSituationActual::GetInstance(UAVObjectManager* objMngr, quint32 instID)
+{
+    return dynamic_cast<FlightSituationActual*>(objMngr->getObject(FlightSituationActual::OBJID, instID));
+}
--- a/ground/src/plugins/uavobjects/flightsituationactual.h
+++ b/ground/src/plugins/uavobjects/flightsituationactual.h
@ -0,0 +1,94 @@
+/**
+ ******************************************************************************
+ *
+ * @file       flightsituationactual.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectsPlugin UAVObjects Plugin
+ * @{
+ *   
+ * @note       Object definition file: flightsituationactual.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @brief      The UAVUObjects GCS plugin 
+ *****************************************************************************/
+/* 
+ * 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 FLIGHTSITUATIONACTUAL_H
+#define FLIGHTSITUATIONACTUAL_H
+
+#include "uavdataobject.h"
+#include "uavobjectmanager.h"
+
+class UAVOBJECTS_EXPORT FlightSituationActual: public UAVDataObject
+{
+    Q_OBJECT
+
+public:
+    // Field structure
+    typedef struct {
+        float Latitude;
+        float Longitude;
+        float Altitude;
+        float ATG;
+        float Climbrate;
+        float Heading;
+        float Airspeed;
+        float Course;
+        float Groundspeed;
+
+    } __attribute__((packed)) DataFields;
+
+    // Field information
+    // Field Latitude information
+    // Field Longitude information
+    // Field Altitude information
+    // Field ATG information
+    // Field Climbrate information
+    // Field Heading information
+    // Field Airspeed information
+    // Field Course information
+    // Field Groundspeed information
+
+  
+    // Constants
+    static const quint32 OBJID = 499301246U;
+    static const QString NAME;
+    static const bool ISSINGLEINST = 1;
+    static const bool ISSETTINGS = 0;
+    static const quint32 NUMBYTES = sizeof(DataFields);
+
+    // Functions
+    FlightSituationActual();
+
+    DataFields getData();
+    void setData(const DataFields& data);
+    Metadata getDefaultMetadata();
+    UAVDataObject* clone(quint32 instID);
+
+    static FlightSituationActual* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
+	
+private:
+    DataFields data;
+
+    void setDefaultFieldValues();
+
+};
+
+#endif // FLIGHTSITUATIONACTUAL_H
--- a/ground/src/plugins/uavobjects/flightsituationactual.py
+++ b/ground/src/plugins/uavobjects/flightsituationactual.py
@ -0,0 +1,166 @@
+##
+##############################################################################
+#
+# @file       flightsituationactual.py
+# @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+# @brief      Implementation of the FlightSituationActual object. This file has been 
+#             automatically generated by the UAVObjectGenerator.
+# 
+# @note       Object definition file: flightsituationactual.xml. 
+#             This is an automatically generated file.
+#             DO NOT modify manually.
+#
+# @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
+#
+
+
+import uavobject
+
+import struct
+from collections import namedtuple
+
+# This is a list of instances of the data fields contained in this object
+_fields = [ \
+	uavobject.UAVObjectField(
+		'Latitude',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Longitude',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Altitude',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'ATG',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Climbrate',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Heading',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Airspeed',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Course',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Groundspeed',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+]
+
+
+class FlightSituationActual(uavobject.UAVObject):
+    ## Object constants
+    OBJID        = 499301246
+    NAME         = "FlightSituationActual"
+    METANAME     = "FlightSituationActualMeta"
+    ISSINGLEINST = 1
+    ISSETTINGS   = 0
+
+    def __init__(self):
+        uavobject.UAVObject.__init__(self,
+                                     self.OBJID, 
+                                     self.NAME,
+                                     self.METANAME,
+                                     0,
+                                     self.ISSINGLEINST)
+
+        for f in _fields:
+            self.add_field(f)
+        
+    def __str__(self):
+        s  = ("0x%08X (%10u)  %-30s  %3u bytes  format '%s'\n"
+                 % (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format))
+        for f in self.get_tuple()._fields:
+            s += ("\t%s\n" % f)
+        return (s)
+
+def main():
+    # Instantiate the object and dump out some interesting info
+    x = FlightSituationActual()
+    print (x)
+
+if __name__ == "__main__":
+    #import pdb ; pdb.run('main()')
+    main()
--- a/ground/src/plugins/uavobjects/navigationdesired.cpp
+++ b/ground/src/plugins/uavobjects/navigationdesired.cpp
@ -0,0 +1,137 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationdesired.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectsPlugin UAVObjects Plugin
+ * @{
+ *   
+ * @note       Object definition file: navigationdesired.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @brief      The UAVUObjects GCS plugin
+ *****************************************************************************/
+/* 
+ * 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 "navigationdesired.h"
+#include "uavobjectfield.h"
+
+const QString NavigationDesired::NAME = QString("NavigationDesired");
+
+/**
+ * Constructor
+ */
+NavigationDesired::NavigationDesired(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
+{
+    // Create fields
+    QList<UAVObjectField*> fields;
+    QStringList LatitudeElemNames;
+    LatitudeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Latitude"), QString("degrees"), UAVObjectField::FLOAT32, LatitudeElemNames, QStringList()) );
+    QStringList LongitudeElemNames;
+    LongitudeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Longitude"), QString("degrees"), UAVObjectField::FLOAT32, LongitudeElemNames, QStringList()) );
+    QStringList AltitudeElemNames;
+    AltitudeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Altitude"), QString("meters"), UAVObjectField::FLOAT32, AltitudeElemNames, QStringList()) );
+    QStringList SpeedElemNames;
+    SpeedElemNames.append("0");
+    fields.append( new UAVObjectField(QString("Speed"), QString("m/s"), UAVObjectField::FLOAT32, SpeedElemNames, QStringList()) );
+
+    // Initialize object
+    initializeFields(fields, (quint8*)&data, NUMBYTES);
+    // Set the default field values
+    setDefaultFieldValues();
+}
+
+/**
+ * Get the default metadata for this object
+ */
+UAVObject::Metadata NavigationDesired::getDefaultMetadata()
+{
+    UAVObject::Metadata metadata;
+    metadata.flightAccess = ACCESS_READWRITE;
+    metadata.gcsAccess = ACCESS_READWRITE;
+    metadata.gcsTelemetryAcked = 0;
+    metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL;
+    metadata.gcsTelemetryUpdatePeriod = 0;
+    metadata.flightTelemetryAcked = 0;
+    metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
+    metadata.flightTelemetryUpdatePeriod = 1000;
+    metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER;
+    metadata.loggingUpdatePeriod = 0;
+    return metadata;
+}
+
+/**
+ * Initialize object fields with the default values.
+ * If a default value is not specified the object fields
+ * will be initialized to zero.
+ */
+void NavigationDesired::setDefaultFieldValues()
+{
+
+}
+
+/**
+ * Get the object data fields
+ */
+NavigationDesired::DataFields NavigationDesired::getData()
+{
+    QMutexLocker locker(mutex);
+    return data;
+}
+
+/**
+ * Set the object data fields
+ */
+void NavigationDesired::setData(const DataFields& data)
+{
+    QMutexLocker locker(mutex);
+    // Get metadata
+    Metadata mdata = getMetadata();
+    // Update object if the access mode permits
+    if ( mdata.gcsAccess == ACCESS_READWRITE )
+    {
+        this->data = data;
+        emit objectUpdatedAuto(this); // trigger object updated event
+        emit objectUpdated(this);
+    }
+}
+
+/**
+ * Create a clone of this object, a new instance ID must be specified.
+ * Do not use this function directly to create new instances, the
+ * UAVObjectManager should be used instead.
+ */
+UAVDataObject* NavigationDesired::clone(quint32 instID)
+{
+    NavigationDesired* obj = new NavigationDesired();
+    obj->initialize(instID, this->getMetaObject());
+    return obj;
+}
+
+/**
+ * Static function to retrieve an instance of the object.
+ */
+NavigationDesired* NavigationDesired::GetInstance(UAVObjectManager* objMngr, quint32 instID)
+{
+    return dynamic_cast<NavigationDesired*>(objMngr->getObject(NavigationDesired::OBJID, instID));
+}
--- a/ground/src/plugins/uavobjects/navigationdesired.h
+++ b/ground/src/plugins/uavobjects/navigationdesired.h
@ -0,0 +1,84 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationdesired.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectsPlugin UAVObjects Plugin
+ * @{
+ *   
+ * @note       Object definition file: navigationdesired.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @brief      The UAVUObjects GCS plugin 
+ *****************************************************************************/
+/* 
+ * 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 NAVIGATIONDESIRED_H
+#define NAVIGATIONDESIRED_H
+
+#include "uavdataobject.h"
+#include "uavobjectmanager.h"
+
+class UAVOBJECTS_EXPORT NavigationDesired: public UAVDataObject
+{
+    Q_OBJECT
+
+public:
+    // Field structure
+    typedef struct {
+        float Latitude;
+        float Longitude;
+        float Altitude;
+        float Speed;
+
+    } __attribute__((packed)) DataFields;
+
+    // Field information
+    // Field Latitude information
+    // Field Longitude information
+    // Field Altitude information
+    // Field Speed information
+
+  
+    // Constants
+    static const quint32 OBJID = 2357718118U;
+    static const QString NAME;
+    static const bool ISSINGLEINST = 1;
+    static const bool ISSETTINGS = 0;
+    static const quint32 NUMBYTES = sizeof(DataFields);
+
+    // Functions
+    NavigationDesired();
+
+    DataFields getData();
+    void setData(const DataFields& data);
+    Metadata getDefaultMetadata();
+    UAVDataObject* clone(quint32 instID);
+
+    static NavigationDesired* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
+	
+private:
+    DataFields data;
+
+    void setDefaultFieldValues();
+
+};
+
+#endif // NAVIGATIONDESIRED_H
--- a/ground/src/plugins/uavobjects/navigationdesired.py
+++ b/ground/src/plugins/uavobjects/navigationdesired.py
@ -0,0 +1,116 @@
+##
+##############################################################################
+#
+# @file       navigationdesired.py
+# @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+# @brief      Implementation of the NavigationDesired object. This file has been 
+#             automatically generated by the UAVObjectGenerator.
+# 
+# @note       Object definition file: navigationdesired.xml. 
+#             This is an automatically generated file.
+#             DO NOT modify manually.
+#
+# @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
+#
+
+
+import uavobject
+
+import struct
+from collections import namedtuple
+
+# This is a list of instances of the data fields contained in this object
+_fields = [ \
+	uavobject.UAVObjectField(
+		'Latitude',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Longitude',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Altitude',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'Speed',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+]
+
+
+class NavigationDesired(uavobject.UAVObject):
+    ## Object constants
+    OBJID        = 2357718118
+    NAME         = "NavigationDesired"
+    METANAME     = "NavigationDesiredMeta"
+    ISSINGLEINST = 1
+    ISSETTINGS   = 0
+
+    def __init__(self):
+        uavobject.UAVObject.__init__(self,
+                                     self.OBJID, 
+                                     self.NAME,
+                                     self.METANAME,
+                                     0,
+                                     self.ISSINGLEINST)
+
+        for f in _fields:
+            self.add_field(f)
+        
+    def __str__(self):
+        s  = ("0x%08X (%10u)  %-30s  %3u bytes  format '%s'\n"
+                 % (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format))
+        for f in self.get_tuple()._fields:
+            s += ("\t%s\n" % f)
+        return (s)
+
+def main():
+    # Instantiate the object and dump out some interesting info
+    x = NavigationDesired()
+    print (x)
+
+if __name__ == "__main__":
+    #import pdb ; pdb.run('main()')
+    main()
--- a/ground/src/plugins/uavobjects/navigationsettings.cpp
+++ b/ground/src/plugins/uavobjects/navigationsettings.cpp
@ -0,0 +1,157 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationsettings.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectsPlugin UAVObjects Plugin
+ * @{
+ *   
+ * @note       Object definition file: navigationsettings.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @brief      The UAVUObjects GCS plugin
+ *****************************************************************************/
+/* 
+ * 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 "navigationsettings.h"
+#include "uavobjectfield.h"
+
+const QString NavigationSettings::NAME = QString("NavigationSettings");
+
+/**
+ * Constructor
+ */
+NavigationSettings::NavigationSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
+{
+    // Create fields
+    QList<UAVObjectField*> fields;
+    QStringList UpdatePeriodElemNames;
+    UpdatePeriodElemNames.append("0");
+    fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT16, UpdatePeriodElemNames, QStringList()) );
+    QStringList AccelerationMaxElemNames;
+    AccelerationMaxElemNames.append("0");
+    fields.append( new UAVObjectField(QString("AccelerationMax"), QString("m/s"), UAVObjectField::FLOAT32, AccelerationMaxElemNames, QStringList()) );
+    QStringList SpeedMaxElemNames;
+    SpeedMaxElemNames.append("0");
+    fields.append( new UAVObjectField(QString("SpeedMax"), QString("m/s"), UAVObjectField::FLOAT32, SpeedMaxElemNames, QStringList()) );
+    QStringList SpeedSafeElemNames;
+    SpeedSafeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("SpeedSafe"), QString("m/s"), UAVObjectField::FLOAT32, SpeedSafeElemNames, QStringList()) );
+    QStringList SpeedMinElemNames;
+    SpeedMinElemNames.append("0");
+    fields.append( new UAVObjectField(QString("SpeedMin"), QString("m/s"), UAVObjectField::FLOAT32, SpeedMinElemNames, QStringList()) );
+    QStringList SettleTimeElemNames;
+    SettleTimeElemNames.append("0");
+    fields.append( new UAVObjectField(QString("SettleTime"), QString("seconds"), UAVObjectField::FLOAT32, SettleTimeElemNames, QStringList()) );
+    QStringList TurnSpeedFactorElemNames;
+    TurnSpeedFactorElemNames.append("0");
+    fields.append( new UAVObjectField(QString("TurnSpeedFactor"), QString(""), UAVObjectField::FLOAT32, TurnSpeedFactorElemNames, QStringList()) );
+    QStringList StabilizationForceFactorElemNames;
+    StabilizationForceFactorElemNames.append("0");
+    fields.append( new UAVObjectField(QString("StabilizationForceFactor"), QString(""), UAVObjectField::FLOAT32, StabilizationForceFactorElemNames, QStringList()) );
+
+    // Initialize object
+    initializeFields(fields, (quint8*)&data, NUMBYTES);
+    // Set the default field values
+    setDefaultFieldValues();
+}
+
+/**
+ * Get the default metadata for this object
+ */
+UAVObject::Metadata NavigationSettings::getDefaultMetadata()
+{
+    UAVObject::Metadata metadata;
+    metadata.flightAccess = ACCESS_READWRITE;
+    metadata.gcsAccess = ACCESS_READWRITE;
+    metadata.gcsTelemetryAcked = 1;
+    metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
+    metadata.gcsTelemetryUpdatePeriod = 0;
+    metadata.flightTelemetryAcked = 1;
+    metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
+    metadata.flightTelemetryUpdatePeriod = 0;
+    metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER;
+    metadata.loggingUpdatePeriod = 0;
+    return metadata;
+}
+
+/**
+ * Initialize object fields with the default values.
+ * If a default value is not specified the object fields
+ * will be initialized to zero.
+ */
+void NavigationSettings::setDefaultFieldValues()
+{
+    data.UpdatePeriod = 100;
+    data.AccelerationMax = 35;
+    data.SpeedMax = 300;
+    data.SpeedSafe = 100;
+    data.SpeedMin = 70;
+    data.SettleTime = 12;
+    data.TurnSpeedFactor = 0.1;
+    data.StabilizationForceFactor = 4;
+
+}
+
+/**
+ * Get the object data fields
+ */
+NavigationSettings::DataFields NavigationSettings::getData()
+{
+    QMutexLocker locker(mutex);
+    return data;
+}
+
+/**
+ * Set the object data fields
+ */
+void NavigationSettings::setData(const DataFields& data)
+{
+    QMutexLocker locker(mutex);
+    // Get metadata
+    Metadata mdata = getMetadata();
+    // Update object if the access mode permits
+    if ( mdata.gcsAccess == ACCESS_READWRITE )
+    {
+        this->data = data;
+        emit objectUpdatedAuto(this); // trigger object updated event
+        emit objectUpdated(this);
+    }
+}
+
+/**
+ * Create a clone of this object, a new instance ID must be specified.
+ * Do not use this function directly to create new instances, the
+ * UAVObjectManager should be used instead.
+ */
+UAVDataObject* NavigationSettings::clone(quint32 instID)
+{
+    NavigationSettings* obj = new NavigationSettings();
+    obj->initialize(instID, this->getMetaObject());
+    return obj;
+}
+
+/**
+ * Static function to retrieve an instance of the object.
+ */
+NavigationSettings* NavigationSettings::GetInstance(UAVObjectManager* objMngr, quint32 instID)
+{
+    return dynamic_cast<NavigationSettings*>(objMngr->getObject(NavigationSettings::OBJID, instID));
+}
--- a/ground/src/plugins/uavobjects/navigationsettings.h
+++ b/ground/src/plugins/uavobjects/navigationsettings.h
@ -0,0 +1,92 @@
+/**
+ ******************************************************************************
+ *
+ * @file       navigationsettings.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @see        The GNU Public License (GPL) Version 3
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectsPlugin UAVObjects Plugin
+ * @{
+ *   
+ * @note       Object definition file: navigationsettings.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @brief      The UAVUObjects GCS plugin 
+ *****************************************************************************/
+/* 
+ * 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 NAVIGATIONSETTINGS_H
+#define NAVIGATIONSETTINGS_H
+
+#include "uavdataobject.h"
+#include "uavobjectmanager.h"
+
+class UAVOBJECTS_EXPORT NavigationSettings: public UAVDataObject
+{
+    Q_OBJECT
+
+public:
+    // Field structure
+    typedef struct {
+        quint16 UpdatePeriod;
+        float AccelerationMax;
+        float SpeedMax;
+        float SpeedSafe;
+        float SpeedMin;
+        float SettleTime;
+        float TurnSpeedFactor;
+        float StabilizationForceFactor;
+
+    } __attribute__((packed)) DataFields;
+
+    // Field information
+    // Field UpdatePeriod information
+    // Field AccelerationMax information
+    // Field SpeedMax information
+    // Field SpeedSafe information
+    // Field SpeedMin information
+    // Field SettleTime information
+    // Field TurnSpeedFactor information
+    // Field StabilizationForceFactor information
+
+  
+    // Constants
+    static const quint32 OBJID = 1350892244U;
+    static const QString NAME;
+    static const bool ISSINGLEINST = 1;
+    static const bool ISSETTINGS = 1;
+    static const quint32 NUMBYTES = sizeof(DataFields);
+
+    // Functions
+    NavigationSettings();
+
+    DataFields getData();
+    void setData(const DataFields& data);
+    Metadata getDefaultMetadata();
+    UAVDataObject* clone(quint32 instID);
+
+    static NavigationSettings* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
+	
+private:
+    DataFields data;
+
+    void setDefaultFieldValues();
+
+};
+
+#endif // NAVIGATIONSETTINGS_H
--- a/ground/src/plugins/uavobjects/navigationsettings.py
+++ b/ground/src/plugins/uavobjects/navigationsettings.py
@ -0,0 +1,156 @@
+##
+##############################################################################
+#
+# @file       navigationsettings.py
+# @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+# @brief      Implementation of the NavigationSettings object. This file has been 
+#             automatically generated by the UAVObjectGenerator.
+# 
+# @note       Object definition file: navigationsettings.xml. 
+#             This is an automatically generated file.
+#             DO NOT modify manually.
+#
+# @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
+#
+
+
+import uavobject
+
+import struct
+from collections import namedtuple
+
+# This is a list of instances of the data fields contained in this object
+_fields = [ \
+	uavobject.UAVObjectField(
+		'UpdatePeriod',
+		'H',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'AccelerationMax',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'SpeedMax',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'SpeedSafe',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'SpeedMin',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'SettleTime',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'TurnSpeedFactor',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+	uavobject.UAVObjectField(
+		'StabilizationForceFactor',
+		'f',
+		1,
+		[
+			'0',
+		],
+		{
+		}
+	),
+]
+
+
+class NavigationSettings(uavobject.UAVObject):
+    ## Object constants
+    OBJID        = 1350892244
+    NAME         = "NavigationSettings"
+    METANAME     = "NavigationSettingsMeta"
+    ISSINGLEINST = 1
+    ISSETTINGS   = 1
+
+    def __init__(self):
+        uavobject.UAVObject.__init__(self,
+                                     self.OBJID, 
+                                     self.NAME,
+                                     self.METANAME,
+                                     0,
+                                     self.ISSINGLEINST)
+
+        for f in _fields:
+            self.add_field(f)
+        
+    def __str__(self):
+        s  = ("0x%08X (%10u)  %-30s  %3u bytes  format '%s'\n"
+                 % (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format))
+        for f in self.get_tuple()._fields:
+            s += ("\t%s\n" % f)
+        return (s)
+
+def main():
+    # Instantiate the object and dump out some interesting info
+    x = NavigationSettings()
+    print (x)
+
+if __name__ == "__main__":
+    #import pdb ; pdb.run('main()')
+    main()
--- a/ground/src/plugins/uavobjects/stabilizationsettings.cpp
+++ b/ground/src/plugins/uavobjects/stabilizationsettings.cpp
@ -124,9 +124,9 @@ void StabilizationSettings::setDefaultFieldValues()
    data.RollKp = 0.02;
    data.RollKi = 4e-06;
    data.RollKd = 0.01;
-    data.YawKp = 1;
-    data.YawKi = 0;
-    data.YawKd = 0;
+    data.YawKp = 0.04;
+    data.YawKi = 4e-06;
+    data.YawKd = 0.01;

 }

--- a/ground/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/src/plugins/uavobjects/uavobjects.pro
@ -27,12 +27,15 @@ HEADERS += uavobjects_global.h \
    telemetrysettings.h \
    systemsettings.h \
    stabilizationsettings.h \
+    flightsituationactual.h \
    manualcontrolsettings.h \
    manualcontrolcommand.h \
    attitudedesired.h \
    actuatorsettings.h \
    actuatordesired.h \
    actuatorcommand.h \
+    navigationsettings.h \
+    navigationdesired.h \
    positionactual.h \
    flightbatterystate.h
 SOURCES += uavobject.cpp \
@ -58,12 +61,15 @@ SOURCES += uavobject.cpp \
    telemetrysettings.cpp \
    systemsettings.cpp \
    stabilizationsettings.cpp \
+    flightsituationactual.cpp \
    manualcontrolsettings.cpp \
    manualcontrolcommand.cpp \
    attitudedesired.cpp \
    actuatorsettings.cpp \
    actuatordesired.cpp \
    actuatorcommand.cpp \
+    navigationsettings.cpp \
+    navigationdesired.cpp \
    positionactual.cpp \
    flightbatterystate.cpp
 OTHER_FILES += UAVObjects.pluginspec
--- a/ground/src/plugins/uavobjects/uavobjectsinit.cpp
+++ b/ground/src/plugins/uavobjects/uavobjectsinit.cpp
@ -42,11 +42,14 @@
 #include "exampleobject2.h"
 #include "examplesettings.h"
 #include "flightbatterystate.h"
+#include "flightsituationactual.h"
 #include "flighttelemetrystats.h"
 #include "gcstelemetrystats.h"
 #include "headingactual.h"
 #include "manualcontrolcommand.h"
 #include "manualcontrolsettings.h"
+#include "navigationdesired.h"
+#include "navigationsettings.h"
 #include "objectpersistence.h"
 #include "positionactual.h"
 #include "stabilizationsettings.h"
@ -74,11 +77,14 @@ void UAVObjectsInitialize(UAVObjectManager* objMngr)
    objMngr->registerObject( new ExampleObject2() );
    objMngr->registerObject( new ExampleSettings() );
    objMngr->registerObject( new FlightBatteryState() );
+    objMngr->registerObject( new FlightSituationActual() );
    objMngr->registerObject( new FlightTelemetryStats() );
    objMngr->registerObject( new GCSTelemetryStats() );
    objMngr->registerObject( new HeadingActual() );
    objMngr->registerObject( new ManualControlCommand() );
    objMngr->registerObject( new ManualControlSettings() );
+    objMngr->registerObject( new NavigationDesired() );
+    objMngr->registerObject( new NavigationSettings() );
    objMngr->registerObject( new ObjectPersistence() );
    objMngr->registerObject( new PositionActual() );
    objMngr->registerObject( new StabilizationSettings() );
--- a/ground/src/shared/uavobjectdefinition/flightsituationactual.xml
+++ b/ground/src/shared/uavobjectdefinition/flightsituationactual.xml
@ -0,0 +1,17 @@
+<xml>
+        <object name="FlightSituationActual" singleinstance="true" settings="false">
+            <field name="Latitude" units="degrees" type="float" elements="1"/>
+            <field name="Longitude" units="degrees" type="float" elements="1"/>
+            <field name="Altitude" units="meters" type="float" elements="1"/>
+            <field name="ATG" units="meters" type="float" elements="1"/>
+            <field name="Climbrate" units="m/s" type="float" elements="1"/>
+            <field name="Heading" units="degrees" type="float" elements="1"/>
+            <field name="Airspeed" units="m/s" type="float" elements="1"/>
+            <field name="Course" units="degrees" type="float" elements="1"/>
+            <field name="Groundspeed" units="m/s" 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>
--- a/ground/src/shared/uavobjectdefinition/navigationdesired.xml
+++ b/ground/src/shared/uavobjectdefinition/navigationdesired.xml
@ -0,0 +1,12 @@
+<xml>
+        <object name="NavigationDesired" singleinstance="true" settings="false">
+            <field name="Latitude" units="degrees" type="float" elements="1"/>
+            <field name="Longitude" units="degrees" type="float" elements="1"/>
+            <field name="Altitude" units="meters" type="float" elements="1"/>
+            <field name="Speed" units="m/s" 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="never" period="0"/>
+        </object>
+</xml>
--- a/ground/src/shared/uavobjectdefinition/navigationsettings.xml
+++ b/ground/src/shared/uavobjectdefinition/navigationsettings.xml
@ -0,0 +1,16 @@
+<xml>
+        <object name="NavigationSettings" singleinstance="true" settings="true">
+            <field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="100"/>
+            <field name="AccelerationMax" units="m/s²" type="float" elements="1" defaultvalue="35"/>
+            <field name="SpeedMax" units="m/s" type="float" elements="1" defaultvalue="300"/>
+            <field name="SpeedSafe" units="m/s" type="float" elements="1" defaultvalue="100"/>
+            <field name="SpeedMin" units="m/s" type="float" elements="1" defaultvalue="70"/>
+            <field name="SettleTime" units="seconds" type="float" elements="1" defaultvalue="12"/>
+            <field name="TurnSpeedFactor" units="" type="float" elements="1" defaultvalue="0.1"/>
+            <field name="StabilizationForceFactor" units="" type="float" elements="1" defaultvalue="4.0"/>
+            <access gcs="readwrite" flight="readwrite"/>
+            <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+            <telemetryflight acked="true" updatemode="onchange" period="0"/>
+            <logging updatemode="never" period="0"/>
+	</object>
+</xml>
--- a/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml
+++ b/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml
@ -10,9 +10,9 @@
            <field name="RollKp" units="" type="float" elements="1" defaultvalue="0.02"/>
            <field name="RollKi" units="" type="float" elements="1" defaultvalue="0.000004"/>
            <field name="RollKd" units="" type="float" elements="1" defaultvalue="0.01"/>
-            <field name="YawKp" units="" type="float" elements="1" defaultvalue="1.0"/>
-            <field name="YawKi" units="" type="float" elements="1" defaultvalue="0.0"/>
-            <field name="YawKd" units="" type="float" elements="1" defaultvalue="0.0"/>
+            <field name="YawKp" units="" type="float" elements="1" defaultvalue="0.04"/>
+            <field name="YawKi" units="" type="float" elements="1" defaultvalue="0.000004"/>
+            <field name="YawKd" units="" type="float" elements="1" defaultvalue="0.01"/>
            <access gcs="readwrite" flight="readwrite"/>
            <telemetrygcs acked="true" updatemode="onchange" period="0"/>
            <telemetryflight acked="true" updatemode="onchange" period="0"/>