Merge branch 'pidt1' into test2

Conflicts: flight/Modules/ManualControl/inc/manualcontrol.h ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro shared/uavobjectdefinition/flightstatus.xml shared/uavobjectdefinition/hwsettings.xml shared/uavobjectdefinition/manualcontrolsettings.xml shared/uavobjectdefinition/taskinfo.xml
2024-12-01 09:24:10 +01:00 · 2012-09-10 14:32:54 -05:00 · 2012-09-10 14:32:54 -05:00 · 38e9d105aa
commit 38e9d105aa
parent 386a9d41ef 4d87af38b6
33 changed files with 2622 additions and 257 deletions
--- a/flight/CopterControl/Makefile
+++ b/flight/CopterControl/Makefile
@ -68,6 +68,7 @@ USE_GPS ?= YES
 USE_TXPID ?= YES
 USE_I2C ?= NO
 USE_ALTITUDE ?= NO
+USE_AUTOTUNE ?= YES
 TEST_FAULTS ?= NO

 # List of optional modules to include
@ -94,6 +95,9 @@ endif
 ifeq ($(TEST_FAULTS), YES)
 OPTMODULES += Fault
 endif
+ifeq ($(USE_AUTOTUNE), YES)
+OPTMODULES += Autotune
+endif

 # List of mandatory modules to include
 MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP
@ -111,6 +115,8 @@ OPTESTS  = ./Tests
 OPMODULEDIR = ../Modules
 FLIGHTLIB = ../Libraries
 FLIGHTLIBINC = $(FLIGHTLIB)/inc
+MATHLIB = ../Libraries/math
+MATHLIBINC = ../Libraries/math
 PIOS = ../PiOS
 PIOSINC = $(PIOS)/inc
 PIOSSTM32F10X = $(PIOS)/STM32F10x
@ -204,6 +210,8 @@ SRC += $(OPUAVSYNTHDIR)/gpssettings.c
 SRC += $(OPUAVSYNTHDIR)/hwsettings.c
 SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
 SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
+SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c
+SRC += $(OPUAVSYNTHDIR)/relaytuning.c
 SRC += $(OPUAVSYNTHDIR)/taskinfo.c
 SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
 SRC += $(OPUAVSYNTHDIR)/ratedesired.c
@ -264,6 +272,8 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
 SRC += $(FLIGHTLIB)/taskmonitor.c
+SRC += $(MATHLIB)/sin_lookup.c
+SRC += $(MATHLIB)/pid.c

 ## CMSIS for STM32
 SRC += $(CMSISDIR)/core_cm3.c
@ -364,6 +374,7 @@ EXTRAINCDIRS  += $(OPUAVSYNTHDIR)
 EXTRAINCDIRS  += $(PIOS)
 EXTRAINCDIRS  += $(PIOSINC)
 EXTRAINCDIRS  += $(FLIGHTLIBINC)
+EXTRAINCDIRS  += $(MATHLIBINC)
 EXTRAINCDIRS  += $(PIOSSTM32F10X)
 EXTRAINCDIRS  += $(PIOSCOMMON)
 EXTRAINCDIRS  += $(PIOSBOARDS)
--- a/flight/Libraries/math/pid.c
+++ b/flight/Libraries/math/pid.c
@ -0,0 +1,101 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Sine and cosine methods that use a cached lookup table
+ * @{
+ *
+ * @file       pid.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Methods to work with PID structure
+ *
+ * @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 "pid.h"
+
+//! Private method
+static float bound(float val, float range);
+
+float pid_apply(struct pid *pid, const float err, float dT)
+{
+	float diff = (err - pid->lastErr);
+	float dterm = 0;
+	pid->lastErr = err;
+	
+	// Scale up accumulator by 1000 while computing to avoid losing precision
+	pid->iAccumulator += err * (pid->i * dT * 1000.0f);
+	pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
+
+	// Calculate DT1 term, fixed T1 timeconstant
+	if(pid->d && dT)
+	{
+		dterm = pid->lastDer +  dT / ( dT + 7.9577e-3f) * ((diff * pid->d / dT) - pid->lastDer);
+		pid->lastDer = dterm;            //   ^ set constant to 1/(2*pi*f_cutoff)
+	}	                                 //   7.9577e-3  means 20 Hz f_cutoff
+ 
+	return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
+}
+
+/**
+ * Reset a bit
+ * @param[in] pid The pid to reset
+ */
+void pid_zero(struct pid *pid)
+{
+	if (!pid)
+		return;
+
+	pid->iAccumulator = 0;
+	pid->lastErr = 0;
+	pid->lastDer = 0;
+}
+
+/**
+ * Configure the settings for a pid structure
+ * @param[out] pid The PID structure to configure
+ * @param[in] p The proportional term
+ * @param[in] i The integral term
+ * @param[in] d The derivative term
+ */
+void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
+{
+	if (!pid)
+		return;
+
+	pid->p = p;
+	pid->i = i;
+	pid->d = d;
+	pid->iLim = iLim;
+}
+
+/**
+ * Bound input value between limits
+ */
+static float bound(float val, float range)
+{
+	if(val < -range) {
+		val = -range;
+	} else if(val > range) {
+		val = range;
+	}
+	return val;
+}
+
--- a/flight/Libraries/math/pid.h
+++ b/flight/Libraries/math/pid.h
@ -0,0 +1,50 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Sine and cosine methods that use a cached lookup table
+ * @{
+ *
+ * @file       pid.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Methods to work with PID structure
+ *
+ * @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 PID_H
+#define PID_H
+
+//! 
+struct pid {
+	float p;
+	float i;
+	float d;
+	float iLim;
+	float iAccumulator;
+	float lastErr;
+	float lastDer;
+};
+
+//! Methods to use the pid structures
+float pid_apply(struct pid *pid, const float err, float dT);
+void pid_zero(struct pid *pid);
+void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
+
+#endif /* PID_H */
--- a/flight/Libraries/math/sin_lookup.c
+++ b/flight/Libraries/math/sin_lookup.c
@ -0,0 +1,142 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Sine and cosine methods that use a cached lookup table
+ * @{
+ *
+ * @file       sin_lookup.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Sine lookup table from flash with 1 degree resolution
+ *
+ * @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 "math.h"
+#include "stdbool.h"
+#include "stdint.h"
+
+#define FLASH_TABLE
+#ifdef FLASH_TABLE
+ /** Version of the code which precomputes the lookup table in flash **/
+
+ // This is a precomputed sin lookup table over 90 degrees that
+ const float sin_table[] = {
+ 	0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f,
+ 	0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f,
+ 	0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f,
+ 	0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f,
+ 	0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f,
+ 	0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f,
+ 	0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f,
+ 	0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f,
+ 	0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f,
+ 	1.000000f,0.999848f,0.999391f,0.998630f,0.997564f,0.996195f,0.994522f,0.992546f,0.990268f,0.987688f,
+ 	0.984808f,0.981627f,0.978148f,0.974370f,0.970296f,0.965926f,0.961262f,0.956305f,0.951057f,0.945519f,
+ 	0.939693f,0.933580f,0.927184f,0.920505f,0.913545f,0.906308f,0.898794f,0.891007f,0.882948f,0.874620f,
+ 	0.866025f,0.857167f,0.848048f,0.838671f,0.829038f,0.819152f,0.809017f,0.798636f,0.788011f,0.777146f,
+ 	0.766044f,0.754710f,0.743145f,0.731354f,0.719340f,0.707107f,0.694658f,0.681998f,0.669131f,0.656059f,
+ 	0.642788f,0.629320f,0.615661f,0.601815f,0.587785f,0.573576f,0.559193f,0.544639f,0.529919f,0.515038f,
+ 	0.500000f,0.484810f,0.469472f,0.453990f,0.438371f,0.422618f,0.406737f,0.390731f,0.374607f,0.358368f,
+ 	0.342020f,0.325568f,0.309017f,0.292372f,0.275637f,0.258819f,0.241922f,0.224951f,0.207912f,0.190809f,
+ 	0.173648f,0.156434f,0.139173f,0.121869f,0.104528f,0.087156f,0.069756f,0.052336f,0.034899f,0.017452f
+ };
+
+int sin_lookup_initalize()
+{
+	return 0;
+}
+
+#else
+/** Version of the code which allocates the lookup table in heap **/
+
+const int SIN_RESOLUTION = 180;
+
+static float *sin_table;
+int sin_lookup_initalize()
+{
+	// Previously initialized
+	if (sin_table)
+		return 0;
+
+	sin_table = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION);
+	if (sin_table == NULL)
+		return -1;
+
+	for(uint32_t i = 0; i < 180; i++)
+		sin_table[i] = sinf((float)i * 2 * M_PI / 360.0f);
+
+	return 0;
+}
+
+#endif
+
+/**
+ * Use the lookup table to return sine(angle) where angle is in radians
+ * to save flash this cheats and uses trig functions to only save
+ * 90 values
+ * @param[in] angle Angle in degrees
+ * @returns sin(angle)
+*/
+float sin_lookup_deg(float angle)
+{
+	if (sin_table == NULL)
+		return 0;
+
+	int i_ang = ((int32_t) angle) % 360;
+	if (i_ang >= 180)          // for 180 to 360 deg
+		return -sin_table[i_ang - 180];
+	else                       // for 0 to 179 deg
+		return sin_table[i_ang];
+
+	return 0;
+}
+
+/**
+ * Get cos(angle) using the sine lookup table
+ * @param[in] angle Angle in degrees
+ * @returns cos(angle)
+ */
+float cos_lookup_deg(float angle)
+{
+	return sin_lookup_deg(angle + 90);
+}
+
+/**
+ * Use the lookup table to return sine(angle) where angle is in radians
+ * @param[in] angle Angle in radians
+ * @returns sin(angle)
+ */
+float sin_lookup_rad(float angle)
+{
+	int degrees = angle * 180.0f / M_PI;
+	return sin_lookup_deg(degrees);
+}
+
+/**
+ * Use the lookup table to return sine(angle) where angle is in radians
+ * @param[in] angle Angle in radians
+ * @returns cos(angle)
+ */
+float cos_lookup_rad(float angle)
+{
+	int degrees = angle * 180.0f / M_PI;
+	return cos_lookup_deg(degrees);
+}
--- a/flight/Libraries/math/sin_lookup.h
+++ b/flight/Libraries/math/sin_lookup.h
@ -0,0 +1,40 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilot Math Utilities
+ * @{
+ * @addtogroup Sine and cosine methods that use a cached lookup table
+ * @{
+ *
+ * @file       sin_lookup.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Sine lookup table from flash with 1 degree resolution
+ *
+ * @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 SIN_LOOKUP_H
+#define SIN_LOOKUP_H
+
+int sin_lookup_initalize();
+float sin_lookup_deg(float angle);
+float cos_lookup_deg(float angle);
+float sin_lookup_rad(float angle);
+float cos_lookup_rad(float angle);
+
+#endif
--- a/flight/Modules/Autotune/autotune.c
+++ b/flight/Modules/Autotune/autotune.c
@ -0,0 +1,333 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup Autotuning module
+ * @brief Reads from @ref ManualControlCommand and fakes a rate mode while 
+ *        toggling the channels to relay mode
+ * @{
+ *
+ * @file       autotune.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Module to handle all comms to the AHRS on a periodic basis.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ ******************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Input objects: None, takes sensor data via pios
+ * Output objects: @ref AttitudeRaw @ref AttitudeActual
+ *
+ * This module computes an attitude estimate from the sensor data
+ *
+ * The module executes in its own thread.
+ *
+ * UAVObjects are automatically generated by the UAVObjectGenerator from
+ * the object definition XML file.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "pios.h"
+#include "flightstatus.h"
+#include "hwsettings.h"
+#include "manualcontrolcommand.h"
+#include "manualcontrolsettings.h"
+#include "relaytuning.h"
+#include "relaytuningsettings.h"
+#include "stabilizationdesired.h"
+#include "stabilizationsettings.h"
+#include <pios_board_info.h>
+ 
+// Private constants
+#define STACK_SIZE_BYTES 1024
+#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
+
+// Private types
+enum AUTOTUNE_STATE {AT_INIT, AT_START, AT_ROLL, AT_PITCH, AT_FINISHED, AT_SET};
+
+// Private variables
+static xTaskHandle taskHandle;
+static bool autotuneEnabled;
+
+// Private functions
+static void AutotuneTask(void *parameters);
+static void update_stabilization_settings();
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AutotuneInitialize(void)
+{
+	// Create a queue, connect to manual control command and flightstatus
+#ifdef MODULE_AUTOTUNE_BUILTIN
+	autotuneEnabled = true;
+#else
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+
+	HwSettingsOptionalModulesGet(optionalModules);
+
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_AUTOTUNE] == HWSETTINGS_OPTIONALMODULES_ENABLED)
+		autotuneEnabled = true;
+	else
+		autotuneEnabled = false;
+#endif
+
+	return 0;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t AutotuneStart(void)
+{
+	// Start main task if it is enabled
+	if(autotuneEnabled) {
+		xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
+
+		TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
+		PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
+	}
+	return 0;
+}
+
+MODULE_INITCALL(AutotuneInitialize, AutotuneStart)
+
+/**
+ * Module thread, should not return.
+ */
+static void AutotuneTask(void *parameters)
+{
+	//AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+	
+	enum AUTOTUNE_STATE state = AT_INIT;
+
+	portTickType lastUpdateTime = xTaskGetTickCount();
+
+	while(1) {
+
+		PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
+		// TODO:
+		// 1. get from queue
+		// 2. based on whether it is flightstatus or manualcontrol
+
+		portTickType diffTime;
+
+		const uint32_t PREPARE_TIME = 2000;
+		const uint32_t MEAURE_TIME = 30000;
+
+		FlightStatusData flightStatus;
+		FlightStatusGet(&flightStatus);
+
+		// Only allow this module to run when autotuning
+		if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
+			state = AT_INIT;
+			vTaskDelay(50);
+			continue;
+		}
+
+		StabilizationDesiredData stabDesired;
+		StabilizationDesiredGet(&stabDesired);
+
+		StabilizationSettingsData stabSettings;
+		StabilizationSettingsGet(&stabSettings);
+
+		ManualControlSettingsData manualSettings;
+		ManualControlSettingsGet(&manualSettings);
+
+		ManualControlCommandData manualControl;
+		ManualControlCommandGet(&manualControl);
+
+		RelayTuningSettingsData relaySettings;
+		RelayTuningSettingsGet(&relaySettings);
+
+		bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE;
+
+		if (rate) { // rate mode
+			stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL]  = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+			stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+
+			stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
+			stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
+		} else {
+			stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL]  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+			stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+
+			stabDesired.Roll = manualControl.Roll * stabSettings.RollMax;
+			stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax;
+		}
+
+		stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW]   = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+		stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
+		stabDesired.Throttle = manualControl.Throttle;
+
+		switch(state) {
+			case AT_INIT:
+
+				lastUpdateTime = xTaskGetTickCount();
+
+				// Only start when armed and flying
+				if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0)
+					state = AT_START;
+				break;
+
+			case AT_START:
+
+				diffTime = xTaskGetTickCount() - lastUpdateTime;
+
+				// Spend the first block of time in normal rate mode to get airborne
+				if (diffTime > PREPARE_TIME) {
+					state = AT_ROLL;
+					lastUpdateTime = xTaskGetTickCount();
+				}
+				break;
+
+			case AT_ROLL:
+
+				diffTime = xTaskGetTickCount() - lastUpdateTime;
+
+				// Run relay mode on the roll axis for the measurement time
+				stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
+					STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
+				if (diffTime > MEAURE_TIME) { // Move on to next state
+					state = AT_PITCH;
+					lastUpdateTime = xTaskGetTickCount();
+				}
+				break;
+
+			case AT_PITCH:
+
+				diffTime = xTaskGetTickCount() - lastUpdateTime;
+
+				// Run relay mode on the pitch axis for the measurement time
+				stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
+					STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
+				if (diffTime > MEAURE_TIME) { // Move on to next state
+					state = AT_FINISHED;
+					lastUpdateTime = xTaskGetTickCount();
+				}
+				break;
+
+			case AT_FINISHED:
+
+				// Wait until disarmed and landed before updating the settings
+				if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0)
+					state = AT_SET;
+
+				break;
+
+			case AT_SET:
+				update_stabilization_settings();
+				state = AT_INIT;
+				break;
+
+			default:
+				// Set an alarm or some shit like that
+				break;
+		}
+
+		StabilizationDesiredSet(&stabDesired);
+
+		vTaskDelay(10);
+	}
+}
+
+/**
+ * Called after measuring roll and pitch to update the
+ * stabilization settings
+ * 
+ * takes in @ref RelayTuning and outputs @ref StabilizationSettings
+ */
+static void update_stabilization_settings()
+{
+	RelayTuningData relayTuning;
+	RelayTuningGet(&relayTuning);
+
+	RelayTuningSettingsData relaySettings;
+	RelayTuningSettingsGet(&relaySettings);
+
+	StabilizationSettingsData stabSettings;
+	StabilizationSettingsGet(&stabSettings);
+
+	// Eventually get these settings from RelayTuningSettings
+	const float gain_ratio_r = 1.0f / 3.0f;
+	const float zero_ratio_r = 1.0f / 3.0f;
+	const float gain_ratio_p = 1.0f / 5.0f;
+	const float zero_ratio_p = 1.0f / 5.0f;
+
+	// For now just run over roll and pitch
+	for (uint i = 0; i < 2; i++) {
+		float wu = 1000.0f * 2 * M_PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s)
+
+		float wc = wu * gain_ratio_r;      // target openloop crossover frequency (rad/s)
+		float zc = wc * zero_ratio_r;      // controller zero location (rad/s)
+		float kpu = 4.0f / M_PI / relayTuning.Gain[i];  // ultimate gain, i.e. the proportional gain for instablity
+		float kp = kpu * gain_ratio_r;     // proportional gain
+		float ki = zc * kp;                // integral gain
+
+		// Now calculate gains for the next loop out knowing it is the integral of
+	 	// the inner loop -- the plant is position/velocity = scale*1/s
+		float wc2 = wc * gain_ratio_p;          // crossover of the attitude loop
+		float kp2 = wc2;                       // kp of attitude
+		float ki2 = wc2 * zero_ratio_p * kp2;  // ki of attitude
+
+		switch(i) {
+			case 0: // roll
+				stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
+				stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
+				stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
+				stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
+				break;
+			case 1: // Pitch
+				stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
+				stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
+				stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
+				stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
+				break;
+			default:
+				// Oh shit oh shit oh shit
+				break;
+		}
+	}
+	switch(relaySettings.Behavior) {
+		case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE:
+			// Just measure, don't update the stab settings
+			break;
+		case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE:
+			StabilizationSettingsSet(&stabSettings);
+			break;
+		case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE:
+			StabilizationSettingsSet(&stabSettings);
+			UAVObjSave(StabilizationSettingsHandle(), 0);
+			break;
+	}
+	
+}
+
+/**
+ * @}
+ * @}
+ */
--- a/flight/Modules/Autotune/inc/autotune.h
+++ b/flight/Modules/Autotune/inc/autotune.h
@ -0,0 +1,37 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup Attitude Attitude Module
+ * @{ 
+ *
+ * @file       attitude.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      Acquires sensor data and fuses it into attitude estimate for CC
+ *
+ * @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 ATTITUDE_H
+#define ATTITUDE_H
+
+#include "openpilot.h"
+
+int32_t AttitudeInitialize(void);
+
+#endif // ATTITUDE_H
--- a/flight/Modules/ManualControl/inc/manualcontrol.h
+++ b/flight/Modules/ManualControl/inc/manualcontrol.h
@ -32,7 +32,7 @@

 #include "manualcontrolcommand.h"

-typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path;
+typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4} flightmode_path;

 #define PARSE_FLIGHT_MODE(x) ( \
 	(x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
@ -43,7 +43,8 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
 	(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \
-	(x == FLIGHTSTATUS_FLIGHTMODE_RTH) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
+	(x == FLIGHTSTATUS_FLIGHTMODE_RTH) ? FLIGHTMODE_GUIDANCE : \
+	(x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : FLIGHTMODE_UNDEFINED \
 	)

 int32_t ManualControlInitialize();
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@ -419,6 +419,10 @@ static void manualControlTask(void *parameters)
 			case FLIGHTMODE_STABILIZED:
 				updateStabilizationDesired(&cmd, &settings);
 				break;
+			case FLIGHTMODE_TUNING:
+				// Tuning takes settings directly from manualcontrolcommand.  No need to
+				// call anything else.  This just avoids errors.
+				break;
 			case FLIGHTMODE_GUIDANCE:
 				switch(flightStatus.FlightMode) {
 					case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
@ -627,6 +631,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
 	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
 	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
 	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
+	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
 	     0; // this is an invalid mode
 					      ;
 	stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
@ -635,6 +641,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
 	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
 	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
 	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
+	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
 	     0; // this is an invalid mode

 	stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
@ -643,6 +651,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
 	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
 	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
 	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
+	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
 	     0; // this is an invalid mode

 	stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
--- a/flight/Modules/Stabilization/inc/relay_tuning.h
+++ b/flight/Modules/Stabilization/inc/relay_tuning.h
@ -0,0 +1,39 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief Relay tuning controller
+ * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
+ * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
+ * @{
+ *
+ * @file       relay_tuning.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @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 RELAY_TUNING_H
+#define RELAY_TUNING_H
+
+int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
+
+#endif
--- a/flight/Modules/Stabilization/inc/stabilization.h
+++ b/flight/Modules/Stabilization/inc/stabilization.h
@ -33,6 +33,8 @@
 #ifndef STABILIZATION_H
 #define STABILIZATION_H

+enum {ROLL,PITCH,YAW,MAX_AXES};
+
 int32_t StabilizationInitialize();

 #endif // STABILIZATION_H
--- a/flight/Modules/Stabilization/inc/virtualflybar.h
+++ b/flight/Modules/Stabilization/inc/virtualflybar.h
@ -0,0 +1,42 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief Virtual flybar mode
+ * @note This file implements the logic for a virtual flybar
+ * @{
+ *
+ * @file       virtualflybar.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @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 VIRTUALFLYBAR_H
+ #define VIRTUALFLYBAR_H
+
+#include "openpilot.h"
+#include "stabilizationsettings.h"
+
+int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings);
+int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT);
+
+ #endif /* VIRTUALFLYBAR_H */
--- a/flight/Modules/Stabilization/relay_tuning.c
+++ b/flight/Modules/Stabilization/relay_tuning.c
@ -0,0 +1,151 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief Relay tuning controller
+ * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
+ * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
+ * @{
+ *
+ * @file       stabilization.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 "relaytuning.h"
+#include "relaytuningsettings.h"
+#include "sin_lookup.h"
+
+//! Private variables
+static const int SIN_RESOLUTION = 180;
+
+#define MAX_AXES 3
+
+/**
+ * Apply a step function for the stabilization controller and monitor the 
+ * result
+ *
+ * Used to  Replace the rate PID with a relay to measure the critical properties of this axis
+ * i.e. period and gain
+ */
+int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
+{
+	RelayTuningData relay;
+	RelayTuningGet(&relay);
+
+	static portTickType lastHighTime;
+	static portTickType lastLowTime;
+
+	static float accum_sin, accum_cos;
+	static uint32_t accumulated = 0;
+
+	const uint16_t DEGLITCH_TIME = 20; // ms
+	const float AMPLITUDE_ALPHA = 0.95;
+	const float PERIOD_ALPHA = 0.95;
+
+	portTickType thisTime = xTaskGetTickCount();
+
+	static bool rateRelayRunning[MAX_AXES];
+
+	// This indicates the current estimate of the smoothed error.  So when it is high
+	// we are waiting for it to go low.
+	static bool high = false;
+
+	// On first run initialize estimates to something reasonable
+	if(reinit) {
+		rateRelayRunning[axis] = false;
+		relay.Period[axis] = 200;
+		relay.Gain[axis] = 0;
+
+		accum_sin = 0;
+		accum_cos = 0;
+		accumulated = 0;
+
+		// These should get reinitialized anyway
+		high = true;
+		lastHighTime = thisTime;
+		lastLowTime = thisTime;
+		RelayTuningSet(&relay);
+	}
+
+
+	RelayTuningSettingsData relaySettings;
+	RelayTuningSettingsGet(&relaySettings);
+
+	// Compute output, simple threshold on error
+	*output = high ? relaySettings.Amplitude : -relaySettings.Amplitude;
+
+	/**** The code below here is to estimate the properties of the oscillation ****/
+
+	// Make sure the period can't go below limit
+	if (relay.Period[axis] < DEGLITCH_TIME)
+		relay.Period[axis] = DEGLITCH_TIME;
+
+	// Project the error onto a sine and cosine of the same frequency
+	// to accumulate the average amplitude
+	int32_t dT = thisTime - lastHighTime;
+	float phase = ((float)360 * (float)dT) / relay.Period[axis];
+	if(phase >= 360)
+		phase = 0;
+	accum_sin += sin_lookup_deg(phase) * error;
+	accum_cos += sin_lookup_deg(phase + 90) * error;
+	accumulated ++;
+
+	// Make sure we've had enough time since last transition then check for a change in the output
+	bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
+
+	if ( !high && time_hysteresis && error > relaySettings.HysteresisThresh ){
+		/* POSITIVE CROSSING DETECTED */
+
+		float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated;
+		float this_gain = this_amplitude / relaySettings.Amplitude;
+
+		accumulated = 0;
+		accum_sin = 0;
+		accum_cos = 0;
+
+		if(rateRelayRunning[axis] == false) {
+			rateRelayRunning[axis] = true;
+			relay.Period[axis] = 200;
+			relay.Gain[axis] = 0;
+		} else {
+			// Low pass filter each amplitude and period
+			relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
+			relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
+		}
+		lastHighTime = thisTime;
+		high = true;
+		RelayTuningSet(&relay);
+
+	} else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) {
+		/* FALLING CROSSING DETECTED */
+
+		lastLowTime = thisTime;
+		high = false;
+
+	}
+
+	return 0;
+}
+
+
--- a/flight/Modules/Stabilization/stabilization.c
+++ b/flight/Modules/Stabilization/stabilization.c
@ -36,12 +36,22 @@
 #include "stabilizationsettings.h"
 #include "actuatordesired.h"
 #include "ratedesired.h"
+#include "relaytuning.h"
+#include "relaytuningsettings.h"
 #include "stabilizationdesired.h"
 #include "attitudeactual.h"
 #include "gyros.h"
 #include "flightstatus.h"
 #include "manualcontrol.h" // Just to get a macro
+
+// Math libraries
 #include "CoordinateConversions.h"
+#include "pid.h"
+#include "sin_lookup.h"
+
+// Includes for various stabilization algorithms
+#include "relay_tuning.h"
+#include "virtualflybar.h"

 // Private constants
 #define MAX_QUEUE_SIZE 1
@ -55,44 +65,26 @@
 #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
 #define FAILSAFE_TIMEOUT_MS 30

-enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_VBAR_ROLL, PID_VBAR_PITCH, PID_VBAR_YAW, PID_MAX};
+enum {PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX};

-enum {ROLL,PITCH,YAW,MAX_AXES};
-
-
-// Private types
-typedef struct {
-	float p;
-	float i;
-	float d;
-	float iLim;
-	float iAccumulator;
-	float lastErr;
-} pid_type;

 // Private variables
 static xTaskHandle taskHandle;
 static StabilizationSettingsData settings;
 static xQueueHandle queue;
 float gyro_alpha = 0;
-float gyro_filtered[3] = {0,0,0};
 float axis_lock_accum[3] = {0,0,0};
-float vbar_sensitivity[3] = {1, 1, 1};
 uint8_t max_axis_lock = 0;
 uint8_t max_axislock_rate = 0;
 float weak_leveling_kp = 0;
 uint8_t weak_leveling_max = 0;
 bool lowThrottleZeroIntegral;
-float vbar_integral[3] = {0, 0, 0};
 float vbar_decay = 0.991f;
-pid_type pids[PID_MAX];
-int8_t vbar_gyros_suppress;
-bool vbar_piro_comp = false;
+struct pid pids[PID_MAX];

 // Private functions
 static void stabilizationTask(void* parameters);
-static float ApplyPid(pid_type * pid, const float err, float dT);
-static float bound(float val);
+static float bound(float val, float range);
 static void ZeroPids(void);
 static void SettingsUpdatedCb(UAVObjEvent * ev);

@ -131,6 +123,11 @@ int32_t StabilizationInitialize()
 	RateDesiredInitialize();
 #endif

+	// Code required for relay tuning
+	sin_lookup_initalize();
+	RelayTuningSettingsInitialize();
+	RelayTuningInitialize();
+
 	return 0;
 }

@ -217,7 +214,7 @@ static void stabilizationTask(void* parameters)
 		local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
 #endif

-
+		float gyro_filtered[3];
 		gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
 		gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
 		gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
@ -226,49 +223,79 @@ static void stabilizationTask(void* parameters)
 		float *actuatorDesiredAxis = &actuatorDesired.Roll;
 		float *rateDesiredAxis = &rateDesired.Roll;

-		//Calculate desired rate
+		ActuatorDesiredGet(&actuatorDesired);
+
+		// A flag to track which stabilization mode each axis is in
+		static uint8_t previous_mode[MAX_AXES] = {255,255,255};
+		bool error = false;
+
+		//Run the selected stabilization algorithm on each axis:
 		for(uint8_t i=0; i< MAX_AXES; i++)
 		{
+			// Check whether this axis mode needs to be reinitialized
+			bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]);
+			previous_mode[i] = stabDesired.StabilizationMode[i];
+
+			// Apply the selected control law
 			switch(stabDesired.StabilizationMode[i])
 			{
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
+					if(reinit)
+						pids[PID_RATE_ROLL + i].iAccumulator = 0;
+
+					// Store to rate desired variable for storing to UAVO
+					rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
+
+					// Compute the inner loop
+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
+
+					break;
+
+				case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
+					if(reinit) {
+						pids[PID_ROLL + i].iAccumulator = 0;
+						pids[PID_RATE_ROLL + i].iAccumulator = 0;
+					}
+
+					// Compute the outer loop
+					rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
+					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
+
+					// Compute the inner loop
+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
+
+					break;
+
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
+
+					// Store for debugging output
 					rateDesiredAxis[i] = attitudeDesiredAxis[i];

-					// Zero attitude and axis lock accumulators
-					pids[PID_ROLL + i].iAccumulator = 0;
-					axis_lock_accum[i] = 0;
-					break;
+					// Run a virtual flybar stabilization algorithm on this axis
+					stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);

+					break;
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
 				{
+					if (reinit)
+						pids[PID_RATE_ROLL + i].iAccumulator = 0;
+
 					float weak_leveling = local_error[i] * weak_leveling_kp;
+					weak_leveling = bound(weak_leveling, weak_leveling_max);

-					if(weak_leveling > weak_leveling_max)
-						weak_leveling = weak_leveling_max;
-					if(weak_leveling < -weak_leveling_max)
-						weak_leveling = -weak_leveling_max;
-
+					// Compute desired rate as input biased towards leveling
 					rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);

-					// Zero attitude and axis lock accumulators
-					pids[PID_ROLL + i].iAccumulator = 0;
-					axis_lock_accum[i] = 0;
 					break;
 				}
-				case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
-					rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
-					
-					if(rateDesiredAxis[i] > settings.MaximumRate[i])
-						rateDesiredAxis[i] = settings.MaximumRate[i];
-					else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
-						rateDesiredAxis[i] = -settings.MaximumRate[i];
-					
-					
-					axis_lock_accum[i] = 0;
-					break;
-
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
+					if (reinit)
+						pids[PID_RATE_ROLL + i].iAccumulator = 0;
+
 					if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
 						// While getting strong commands act like rate mode
 						rateDesiredAxis[i] = attitudeDesiredAxis[i];
@ -276,154 +303,96 @@ static void stabilizationTask(void* parameters)
 					} else {
 						// For weaker commands or no command simply attitude lock (almost) on no gyro change
 						axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
-						if(axis_lock_accum[i] > max_axis_lock)
-							axis_lock_accum[i] = max_axis_lock;
-						else if(axis_lock_accum[i] < -max_axis_lock)
-							axis_lock_accum[i] = -max_axis_lock;
-
-						rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
+						axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
+						rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
 					}

-					if(rateDesiredAxis[i] > settings.MaximumRate[i])
-						rateDesiredAxis[i] = settings.MaximumRate[i];
-					else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
-						rateDesiredAxis[i] = -settings.MaximumRate[i];
+					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);

+					actuatorDesiredAxis[i] = pid_apply(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
+					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
+
+					break;
+
+				case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
+					// Store to rate desired variable for storing to UAVO
+					rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
+
+					// Run the relay controller which also estimates the oscillation parameters
+					stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
+					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0);
+
+					break;
+
+				case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
+					if(reinit)
+						pids[PID_ROLL + i].iAccumulator = 0;
+
+					// Compute the outer loop like attitude mode
+					rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
+					rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
+
+					// Run the relay controller which also estimates the oscillation parameters
+					stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
+					actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0);
+
+					break;
+
+				case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
+					actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f);
+					break;
+				default:
+					error = true;
 					break;
 			}
 		}

-		// Piro compensation rotates the virtual flybar by yaw step to keep it
-		// rotated to external world
-		if (vbar_piro_comp) {
-			const float F_PI = 3.14159f;
-			float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT);
-			float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT);
+		if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE)
+			stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);

-			float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
-			float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
-			
-			vbar_integral[1] = vbar_pitch;
-			vbar_integral[0] = vbar_roll;
-		}
-
-		uint8_t shouldUpdate = 1;
 #if defined(DIAGNOSTICS)
 		RateDesiredSet(&rateDesired);
 #endif
-		ActuatorDesiredGet(&actuatorDesired);
-		//Calculate desired command
-		for(uint32_t i = 0; i < MAX_AXES; i++)
-		{
-			switch(stabDesired.StabilizationMode[i])
-			{
-				case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
-				case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
-				case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
-				case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
-				{
-					float command = ApplyPid(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i] - gyro_filtered[i], dT);
-					actuatorDesiredAxis[i] = bound(command);
-					break;
-				}
-				case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
-				{
-					// Track the angle of the virtual flybar which includes a slow decay
-					vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
-					if (vbar_integral[i] > settings.VbarMaxAngle)
-						vbar_integral[i] = settings.VbarMaxAngle;
-					if (-vbar_integral[i] < -settings.VbarMaxAngle)
-						vbar_integral[i] = -settings.VbarMaxAngle;
-
-					// Command signal is composed of stick input added to the gyro and virtual flybar
-					float gyro_gain = 1.0f;
-					if (vbar_gyros_suppress > 0) {
-						gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f);
-						gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
-					}
-					float command = rateDesiredAxis[i] * vbar_sensitivity[i] - gyro_gain * (
-							vbar_integral[i] * pids[PID_VBAR_ROLL + i].i +
-							gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p);
-
-					actuatorDesiredAxis[i] = bound(command);
-					break;
-				}
-				case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
-					switch (i)
-				{
-					case ROLL:
-						actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
-						shouldUpdate = 1;
-						pids[PID_RATE_ROLL].iAccumulator = 0;
-						pids[PID_ROLL].iAccumulator = 0;
-						break;
-					case PITCH:
-						actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
-						shouldUpdate = 1;
-						pids[PID_RATE_PITCH].iAccumulator = 0;
-						pids[PID_PITCH].iAccumulator = 0;
-						break;
-					case YAW:
-						actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
-						shouldUpdate = 1;
-						pids[PID_RATE_YAW].iAccumulator = 0;
-						pids[PID_YAW].iAccumulator = 0;
-						break;
-				}
-					break;
-
-			}
-		}

 		// Save dT
 		actuatorDesired.UpdateTime = dT * 1000;
+		actuatorDesired.Throttle = stabDesired.Throttle;

-		if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
-			shouldUpdate = 0;
-
-		if(shouldUpdate)
-		{
-			actuatorDesired.Throttle = stabDesired.Throttle;
-			if(dT > 15)
-				actuatorDesired.NumLongUpdates++;
+		if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
 			ActuatorDesiredSet(&actuatorDesired);
+		} else {
+			// Force all axes to reinitialize when engaged
+			for(uint8_t i=0; i< MAX_AXES; i++)
+				previous_mode[i] = 255;
 		}

 		if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
-		   (lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
-		   !shouldUpdate)
+		   (lowThrottleZeroIntegral && stabDesired.Throttle < 0))
 		{
-			ZeroPids();
+			// Force all axes to reinitialize when engaged
+			for(uint8_t i=0; i< MAX_AXES; i++)
+				previous_mode[i] = 255;
 		}

-
-		// Clear alarms
-		AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
+		// Clear or set alarms.  Done like this to prevent toggline each cycle
+		// and hammering system alarms
+		if (error)
+			AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR);
+		else
+			AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
 	}
 }

-float ApplyPid(pid_type * pid, const float err, float dT)
-{
-	float diff = (err - pid->lastErr);
-	pid->lastErr = err;
-
-	// Scale up accumulator by 1000 while computing to avoid losing precision
-	pid->iAccumulator += err * (pid->i * dT * 1000.0f);
-	if(pid->iAccumulator > (pid->iLim * 1000.0f)) {
-		pid->iAccumulator = pid->iLim * 1000.0f;
-	} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
-		pid->iAccumulator = -pid->iLim * 1000.0f;
-	}
-	return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
-}
-

+/**
+ * Clear the accumulators and derivatives for all the axes
+ */
 static void ZeroPids(void)
 {
-	for(int8_t ct = 0; ct < PID_MAX; ct++) {
-		pids[ct].iAccumulator = 0.0f;
-		pids[ct].lastErr = 0.0f;
-	}
+	for(uint32_t i = 0; i < PID_MAX; i++)
+		pid_zero(&pids[i]);
+
+
 	for(uint8_t i = 0; i < 3; i++)
 		axis_lock_accum[i] = 0.0f;
 }
@ -432,12 +401,12 @@ static void ZeroPids(void)
 /**
 * Bound input value between limits
 */
-static float bound(float val)
+static float bound(float val, float range)
 {
-	if(val < -1.0f) {
-		val = -1.0f;
-	} else if(val > 1.0f) {
-		val = 1.0f;
+	if(val < -range) {
+		val = -range;
+	} else if(val > range) {
+		val = range;
 	}
 	return val;
 }
@ -447,54 +416,37 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
 	StabilizationSettingsGet(&settings);
 	
 	// Set the roll rate PID constants
-	pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
-	pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
-	pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
-	pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
+	pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], 
+		settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
+		pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD],
+		pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);
 	
 	// Set the pitch rate PID constants
-	pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
-	pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
-	pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
-	pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
+	pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], 
+		pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
+		pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD],
+		pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);
 	
 	// Set the yaw rate PID constants
-	pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
-	pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
-	pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
-	pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
+	pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP],
+		pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
+		pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD],
+		pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);
 	
 	// Set the roll attitude PI constants
-	pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
-	pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
-	pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
+	pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP],
+		settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
+		pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);
 	
 	// Set the pitch attitude PI constants
-	pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
-	pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
-	pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
+	pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP],
+		pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
+		settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);
 	
 	// Set the yaw attitude PI constants
-	pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
-	pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
-	pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
-
-	// Set the roll attitude PI constants
-	pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
-	pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
-	
-	// Set the pitch attitude PI constants
-	pids[PID_VBAR_PITCH].p = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KP];
-	pids[PID_VBAR_PITCH].i = settings.VbarPitchPI[STABILIZATIONSETTINGS_VBARPITCHPI_KI];
-	
-	// Set the yaw attitude PI constants
-	pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP];
-	pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI];
-
-	// Need to store the vbar sensitivity
-	vbar_sensitivity[0] = settings.VbarSensitivity[0];
-	vbar_sensitivity[1] = settings.VbarSensitivity[1];
-	vbar_sensitivity[2] = settings.VbarSensitivity[2];
+	pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP],
+		settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
+		settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]);
 	
 	// Maximum deviation to accumulate for axis lock
 	max_axis_lock = settings.MaxAxisLock;
@ -520,8 +472,6 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
 	
 	// Compute time constant for vbar decay term based on a tau
 	vbar_decay = expf(-fakeDt / settings.VbarTau);
-	vbar_gyros_suppress = settings.VbarGyroSuppress;
-	vbar_piro_comp = settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE;
 }


@ -529,3 +479,4 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
 * @}
 * @}
 */
+
--- a/flight/Modules/Stabilization/virtualflybar.c
+++ b/flight/Modules/Stabilization/virtualflybar.c
@ -0,0 +1,119 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup StabilizationModule Stabilization Module
+ * @brief Virtual flybar mode
+ * @note This file implements the logic for a virtual flybar
+ * @{
+ *
+ * @file       virtualflybar.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @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 "stabilization.h"
+#include "stabilizationsettings.h"
+
+//! Private variables
+static float vbar_integral[MAX_AXES];
+static float vbar_decay = 0.991f;
+
+//! Private methods
+static float bound(float val, float range);
+
+int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
+{
+	float gyro_gain = 1.0f;
+	float kp = 0, ki = 0;
+
+	if(reinit)
+		vbar_integral[axis] = 0;
+
+	// Track the angle of the virtual flybar which includes a slow decay
+	vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
+	vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle);
+
+	// Command signal can indicate how much to disregard the gyro feedback (fast flips)
+	if (settings->VbarGyroSuppress > 0) {
+		gyro_gain = (1.0f - fabs(command) * settings->VbarGyroSuppress / 100.0f);
+		gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
+	}
+
+	// Get the settings for the correct axis
+	switch(axis)
+	{
+		case ROLL:
+			kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
+			ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
+			break;
+		case PITCH:
+			kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
+			ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
+			break;
+		case YAW:
+			kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
+			ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
+			break;
+		default:
+			PIOS_DEBUG_Assert(0);
+	}
+
+	// Command signal is composed of stick input added to the gyro and virtual flybar
+	*output = command * settings->VbarSensitivity[axis] - 
+	    gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
+
+	return 0;
+}
+
+/**
+ * Want to keep the virtual flybar fixed in world coordinates as we pirouette
+ * @param[in] z_gyro The deg/s of rotation along the z axis
+ * @param[in] dT The time since last sample
+ */
+int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
+{
+	const float F_PI = (float) M_PI;
+	float cy = cosf(z_gyro / 180.0f * F_PI * dT);
+	float sy = sinf(z_gyro / 180.0f * F_PI * dT);
+
+	float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
+	float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
+
+	vbar_integral[1] = vbar_pitch;
+	vbar_integral[0] = vbar_roll;
+
+	return 0;
+}
+
+/**
+ * Bound input value between limits
+ */
+static float bound(float val, float range)
+{
+	if(val < -range) {
+		val = -range;
+	} else if(val > range) {
+		val = range;
+	}
+	return val;
+}
--- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h
+++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h
@ -74,6 +74,7 @@ TIM4  |  RC In 1  |  Servo 3  |  Servo 2  |  Servo 1
 #define PIOS_WDG_STABILIZATION   0x0002
 #define PIOS_WDG_ATTITUDE        0x0004
 #define PIOS_WDG_MANUAL          0x0008
+#define PIOS_WDG_AUTOTUNE        0x0010

 //------------------------
 // TELEMETRY
--- a/flight/RevoMini/Makefile
+++ b/flight/RevoMini/Makefile
@ -75,6 +75,8 @@ PIOSINC = $(PIOS)/inc
 OPMODULEDIR = ../Modules
 FLIGHTLIB = ../Libraries
 FLIGHTLIBINC = ../Libraries/inc
+MATHLIB = ../Libraries/math
+MATHLIBINC = ../Libraries/math
 RSCODE = $(FLIGHTLIB)/rscode
 RSCODEINC = $(FLIGHTLIB)/rscode
 PIOSSTM32F4XX = $(PIOS)/STM32F4xx
@ -144,6 +146,8 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/WorldMagModel.c
 SRC += $(FLIGHTLIB)/insgps13state.c
 SRC += $(FLIGHTLIB)/taskmonitor.c
+SRC += $(MATHLIB)/sin_lookup.c
+SRC += $(MATHLIB)/pid.c

 ## For RFM22b
 SRC += $(FLIGHTLIB)/packet_handler.c
@ -214,6 +218,7 @@ EXTRAINCDIRS  += $(OPUAVOBJ)
 EXTRAINCDIRS  += $(OPUAVOBJINC)
 EXTRAINCDIRS  += $(UAVOBJSYNTHDIR)
 EXTRAINCDIRS  += $(FLIGHTLIBINC)
+EXTRAINCDIRS  += $(MATHLIBINC)
 EXTRAINCDIRS  += $(RSCODEINC)
 EXTRAINCDIRS  += $(PIOSSTM32F4XX)
 EXTRAINCDIRS  += $(PIOSCOMMON)
--- a/flight/RevoMini/UAVObjects.inc
+++ b/flight/RevoMini/UAVObjects.inc
@ -72,6 +72,8 @@ UAVOBJSRCFILENAMES += pipxstatus
 UAVOBJSRCFILENAMES += pipxsettings
 UAVOBJSRCFILENAMES += positionactual
 UAVOBJSRCFILENAMES += ratedesired
+UAVOBJSRCFILENAMES += relaytuning
+UAVOBJSRCFILENAMES += relaytuningsettings
 UAVOBJSRCFILENAMES += revocalibration
 UAVOBJSRCFILENAMES += revosettings
 UAVOBJSRCFILENAMES += sonaraltitude
--- a/flight/Revolution/UAVObjects.inc
+++ b/flight/Revolution/UAVObjects.inc
@ -70,6 +70,8 @@ UAVOBJSRCFILENAMES += pathplannersettings
 UAVOBJSRCFILENAMES += pathdesired
 UAVOBJSRCFILENAMES += positionactual
 UAVOBJSRCFILENAMES += ratedesired
+UAVOBJSRCFILENAMES += relaytuning
+UAVOBJSRCFILENAMES += relaytuningsettings
 UAVOBJSRCFILENAMES += revocalibration
 UAVOBJSRCFILENAMES += revosettings
 UAVOBJSRCFILENAMES += sonaraltitude
--- a/ground/openpilotgcs/src/plugins/config/autotune.ui
+++ b/ground/openpilotgcs/src/plugins/config/autotune.ui
@ -0,0 +1,1084 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>AutotuneWidget</class>
+ <widget class="QWidget" name="AutotuneWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>443</width>
+    <height>575</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <widget class="QGroupBox" name="groupBox">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>10</y>
+     <width>401</width>
+     <height>131</height>
+    </rect>
+   </property>
+   <property name="title">
+    <string>Tuning Aggressiveness</string>
+   </property>
+   <layout class="QGridLayout" name="gridLayout_2">
+    <item row="0" column="0">
+     <widget class="QLabel" name="label">
+      <property name="text">
+       <string>Rate Tuning:</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="0">
+     <widget class="QLabel" name="label_2">
+      <property name="text">
+       <string>Attitude Tuning:</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="1">
+     <widget class="QSlider" name="rateTuning">
+      <property name="maximum">
+       <number>100</number>
+      </property>
+      <property name="orientation">
+       <enum>Qt::Horizontal</enum>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>objname:RelayTuningSettings</string>
+        <string>fieldname:RateGain</string>
+        <string>scale:0.01</string>
+        <string>haslimits:no</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="1">
+     <widget class="QSlider" name="attitudeTuning">
+      <property name="maximum">
+       <number>100</number>
+      </property>
+      <property name="orientation">
+       <enum>Qt::Horizontal</enum>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>objname:RelayTuningSettings</string>
+        <string>fieldname:AttitudeGain</string>
+        <string>scale:0.01</string>
+        <string>haslimits:no</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QGroupBox" name="groupBox_2">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>250</y>
+     <width>401</width>
+     <height>121</height>
+    </rect>
+   </property>
+   <property name="title">
+    <string>Computed Values</string>
+   </property>
+   <layout class="QGridLayout" name="gridLayout_3">
+    <item row="0" column="1">
+     <widget class="QLabel" name="label_5">
+      <property name="text">
+       <string>RateKp</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="0">
+     <widget class="QLabel" name="label_4">
+      <property name="text">
+       <string>Roll</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="2">
+     <widget class="QLabel" name="label_9">
+      <property name="text">
+       <string>RateKi</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="3">
+     <widget class="QLabel" name="label_10">
+      <property name="text">
+       <string>AttitudeKp</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="4">
+     <widget class="QLabel" name="label_11">
+      <property name="text">
+       <string>AttitudeKi</string>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="0">
+     <widget class="QLabel" name="label_12">
+      <property name="text">
+       <string>Pitch</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="1">
+     <widget class="QLabel" name="rollRateKp">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="1">
+     <widget class="QLabel" name="pitchRateKp">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="2">
+     <widget class="QLabel" name="rollRateKi">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="2">
+     <widget class="QLabel" name="pitchRateKi">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="3">
+     <widget class="QLabel" name="rollAttitudeKp">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="3">
+     <widget class="QLabel" name="pitchAttitudeKp">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="4">
+     <widget class="QLabel" name="rollAttitudeKi">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="4">
+     <widget class="QLabel" name="pitchAttitudeKi">
+      <property name="text">
+       <string>0</string>
+      </property>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QGroupBox" name="groupBox_3">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>140</y>
+     <width>401</width>
+     <height>111</height>
+    </rect>
+   </property>
+   <property name="title">
+    <string>Measured Properties</string>
+   </property>
+   <layout class="QGridLayout" name="gridLayout">
+    <item row="1" column="0">
+     <widget class="QLabel" name="label_3">
+      <property name="text">
+       <string>Roll:</string>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="1">
+     <widget class="QLabel" name="measuredRollPeriod">
+      <property name="text">
+       <string>0</string>
+      </property>
+      <property name="alignment">
+       <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>objname:RelayTuning</string>
+        <string>fieldname:Period</string>
+        <string>element:Roll</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+    <item row="1" column="2">
+     <widget class="QLabel" name="measuredRollGain">
+      <property name="text">
+       <string>0</string>
+      </property>
+      <property name="alignment">
+       <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>objname:RelayTuning</string>
+        <string>fieldname:Gain</string>
+        <string>element:Roll</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="1">
+     <widget class="QLabel" name="label_6">
+      <property name="text">
+       <string>Period (ms)</string>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="2">
+     <widget class="QLabel" name="label_7">
+      <property name="text">
+       <string>Gain (deg/s) / output</string>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="0">
+     <widget class="QLabel" name="label_8">
+      <property name="text">
+       <string>Pitch</string>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="1">
+     <widget class="QLabel" name="measuredPitchPeriod">
+      <property name="text">
+       <string>0</string>
+      </property>
+      <property name="alignment">
+       <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>objname:RelayTuning</string>
+        <string>fieldname:Period</string>
+        <string>element:Pitch</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+    <item row="2" column="2">
+     <widget class="QLabel" name="measuredPitchGain">
+      <property name="text">
+       <string>0</string>
+      </property>
+      <property name="alignment">
+       <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>objname:RelayTuning</string>
+        <string>fieldname:Gain</string>
+        <string>element:Pitch</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QGroupBox" name="RateStabilizationGroup_21">
+   <property name="geometry">
+    <rect>
+     <x>20</x>
+     <y>480</y>
+     <width>401</width>
+     <height>79</height>
+    </rect>
+   </property>
+   <property name="sizePolicy">
+    <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+     <horstretch>0</horstretch>
+     <verstretch>0</verstretch>
+    </sizepolicy>
+   </property>
+   <property name="minimumSize">
+    <size>
+     <width>0</width>
+     <height>79</height>
+    </size>
+   </property>
+   <property name="maximumSize">
+    <size>
+     <width>16777215</width>
+     <height>79</height>
+    </size>
+   </property>
+   <property name="palette">
+    <palette>
+     <active>
+      <colorrole role="WindowText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Button">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Light">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>255</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Midlight">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>251</red>
+         <green>251</green>
+         <blue>251</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Dark">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>124</red>
+         <green>124</green>
+         <blue>124</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Mid">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>165</red>
+         <green>165</green>
+         <blue>165</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Text">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="BrightText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>255</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ButtonText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Base">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Window">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Shadow">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="AlternateBase">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>251</red>
+         <green>251</green>
+         <blue>251</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ToolTipBase">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>220</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ToolTipText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+     </active>
+     <inactive>
+      <colorrole role="WindowText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Button">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Light">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>255</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Midlight">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>251</red>
+         <green>251</green>
+         <blue>251</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Dark">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>124</red>
+         <green>124</green>
+         <blue>124</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Mid">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>165</red>
+         <green>165</green>
+         <blue>165</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Text">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="BrightText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>255</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ButtonText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Base">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Window">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Shadow">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="AlternateBase">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>251</red>
+         <green>251</green>
+         <blue>251</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ToolTipBase">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>220</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ToolTipText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+     </inactive>
+     <disabled>
+      <colorrole role="WindowText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>124</red>
+         <green>124</green>
+         <blue>124</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Button">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Light">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>255</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Midlight">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>251</red>
+         <green>251</green>
+         <blue>251</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Dark">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>124</red>
+         <green>124</green>
+         <blue>124</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Mid">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>165</red>
+         <green>165</green>
+         <blue>165</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Text">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>124</red>
+         <green>124</green>
+         <blue>124</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="BrightText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>255</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ButtonText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>124</red>
+         <green>124</green>
+         <blue>124</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="Base">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Window">
+       <brush brushstyle="LinearGradientPattern">
+        <gradient startx="0.507000000000000" starty="0.869318000000000" endx="0.507000000000000" endy="0.096590900000000" type="LinearGradient" spread="PadSpread" coordinatemode="ObjectBoundingMode">
+         <gradientstop position="0.000000000000000">
+          <color alpha="255">
+           <red>243</red>
+           <green>243</green>
+           <blue>243</blue>
+          </color>
+         </gradientstop>
+         <gradientstop position="1.000000000000000">
+          <color alpha="255">
+           <red>250</red>
+           <green>250</green>
+           <blue>250</blue>
+          </color>
+         </gradientstop>
+        </gradient>
+       </brush>
+      </colorrole>
+      <colorrole role="Shadow">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="AlternateBase">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>248</red>
+         <green>248</green>
+         <blue>248</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ToolTipBase">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>255</red>
+         <green>255</green>
+         <blue>220</blue>
+        </color>
+       </brush>
+      </colorrole>
+      <colorrole role="ToolTipText">
+       <brush brushstyle="SolidPattern">
+        <color alpha="255">
+         <red>0</red>
+         <green>0</green>
+         <blue>0</blue>
+        </color>
+       </brush>
+      </colorrole>
+     </disabled>
+    </palette>
+   </property>
+   <property name="autoFillBackground">
+    <bool>false</bool>
+   </property>
+   <property name="title">
+    <string/>
+   </property>
+   <property name="alignment">
+    <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+   </property>
+   <layout class="QGridLayout" name="gridLayout_10">
+    <item row="0" column="0">
+     <spacer name="horizontalSpacer_3">
+      <property name="orientation">
+       <enum>Qt::Horizontal</enum>
+      </property>
+      <property name="sizeHint" stdset="0">
+       <size>
+        <width>111</width>
+        <height>10</height>
+       </size>
+      </property>
+     </spacer>
+    </item>
+    <item row="0" column="1">
+     <widget class="QPushButton" name="stabilizationReloadBoardData_6">
+      <property name="minimumSize">
+       <size>
+        <width>120</width>
+        <height>28</height>
+       </size>
+      </property>
+      <property name="toolTip">
+       <string>Reloads the saved settings into GCS.
+Useful if you have accidentally changed some settings.</string>
+      </property>
+      <property name="styleSheet">
+       <string notr="true">QPushButton {
+border: 1px outset #999;
+border-radius: 5;
+background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
+}
+
+QPushButton:pressed {
+
+    border-style: inset;
+	background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
+
+}
+
+
+QPushButton:hover  {
+	border: 1px outset #999;	
+border-color: rgb(83, 83, 83);
+border-radius: 4;
+	}</string>
+      </property>
+      <property name="text">
+       <string>Reload Board Data</string>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>button:reload</string>
+        <string>buttongroup:10</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="2">
+     <widget class="QPushButton" name="saveStabilizationToRAM_6">
+      <property name="minimumSize">
+       <size>
+        <width>60</width>
+        <height>28</height>
+       </size>
+      </property>
+      <property name="toolTip">
+       <string>Send settings to the board but do not save to the non-volatile memory</string>
+      </property>
+      <property name="styleSheet">
+       <string notr="true">QPushButton {
+border: 1px outset #999;
+border-radius: 5;
+background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
+}
+
+QPushButton:pressed {
+
+    border-style: inset;
+	background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
+
+}
+
+
+QPushButton:hover  {
+	border: 1px outset #999;	
+border-color: rgb(83, 83, 83);
+border-radius: 4;
+	}</string>
+      </property>
+      <property name="text">
+       <string>Apply</string>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>button:apply</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+    <item row="0" column="3">
+     <widget class="QPushButton" name="saveStabilizationToSD_6">
+      <property name="minimumSize">
+       <size>
+        <width>60</width>
+        <height>28</height>
+       </size>
+      </property>
+      <property name="toolTip">
+       <string>Send settings to the board and save to the non-volatile memory</string>
+      </property>
+      <property name="styleSheet">
+       <string notr="true">QPushButton {
+border: 1px outset #999;
+border-radius: 5;
+background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
+}
+
+QPushButton:pressed {
+
+    border-style: inset;
+	background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
+
+}
+
+
+QPushButton:hover  {
+	border: 1px outset #999;	
+border-color: rgb(83, 83, 83);
+border-radius: 4;
+	}</string>
+      </property>
+      <property name="text">
+       <string>Save</string>
+      </property>
+      <property name="objrelation" stdset="0">
+       <stringlist>
+        <string>button:save</string>
+       </stringlist>
+      </property>
+     </widget>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QLabel" name="label_22">
+   <property name="geometry">
+    <rect>
+     <x>30</x>
+     <y>440</y>
+     <width>371</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>The buttons below change the autotuning settings which
+will alter thingsfor the next autotuning flight</string>
+   </property>
+  </widget>
+  <widget class="QPushButton" name="useComputedValues">
+   <property name="geometry">
+    <rect>
+     <x>250</x>
+     <y>380</y>
+     <width>171</width>
+     <height>31</height>
+    </rect>
+   </property>
+   <property name="styleSheet">
+    <string notr="true">QPushButton {
+border: 1px outset #999;
+border-radius: 5;
+background-color: qlineargradient(spread:pad, x1:0.507, y1:0.869318, x2:0.507, y2:0.0965909, stop:0.28 rgba(236, 236, 236, 255), stop:1 rgba(252, 252, 252, 255));
+}
+
+QPushButton:pressed {
+
+    border-style: inset;
+	background-color: qlineargradient(spread:pad, x1:0.502, y1:0.664864, x2:0.502, y2:0.034, stop:0.358209 rgba(250, 250, 250, 255), stop:0.626866 rgba(235, 235, 235, 255));
+
+}
+
+
+QPushButton:hover  {
+	border: 1px outset #999;	
+border-color: rgb(83, 83, 83);
+border-radius: 4;
+	}</string>
+   </property>
+   <property name="text">
+    <string>Apply Computed Values</string>
+   </property>
+  </widget>
+  <widget class="QLabel" name="label_21">
+   <property name="geometry">
+    <rect>
+     <x>40</x>
+     <y>410</y>
+     <width>104</width>
+     <height>20</height>
+    </rect>
+   </property>
+   <property name="text">
+    <string>Step Size</string>
+   </property>
+  </widget>
+  <widget class="QSlider" name="stepSizeSlider">
+   <property name="geometry">
+    <rect>
+     <x>149</x>
+     <y>410</y>
+     <width>266</width>
+     <height>20</height>
+    </rect>
+   </property>
+   <property name="orientation">
+    <enum>Qt::Horizontal</enum>
+   </property>
+   <property name="objrelation" stdset="0">
+    <stringlist>
+     <string>objname:RelayTuningSettings</string>
+     <string>fieldname:Amplitude</string>
+     <string>scale:0.01</string>
+     <string>haslimits:no</string>
+    </stringlist>
+   </property>
+  </widget>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
--- a/ground/openpilotgcs/src/plugins/config/config.pro
+++ b/ground/openpilotgcs/src/plugins/config/config.pro
@ -36,7 +36,8 @@ HEADERS += configplugin.h \
    cfg_vehicletypes/configfixedwingwidget.h \
    cfg_vehicletypes/vehicleconfig.h \
    configrevowidget.h \
-    config_global.h
+    config_global.h \
+    configautotunewidget.h
 SOURCES += configplugin.cpp \
    configgadgetconfiguration.cpp \
    configgadgetwidget.cpp \
@ -67,7 +68,8 @@ SOURCES += configplugin.cpp \
    cfg_vehicletypes/configfixedwingwidget.cpp \
    cfg_vehicletypes/configccpmwidget.cpp \
    outputchannelform.cpp \
-    cfg_vehicletypes/vehicleconfig.cpp
+    cfg_vehicletypes/vehicleconfig.cpp \
+    configautotunewidget.cpp
 FORMS += airframe.ui \
    cc_hw_settings.ui \
    pro_hw_settings.ui \
@ -83,5 +85,13 @@ FORMS += airframe.ui \
    outputchannelform.ui \
    revosensors.ui \
    txpid.ui \
-    pipxtreme.ui
+    pipxtreme.ui \
+    autotune.ui
 RESOURCES += configgadget.qrc
+
+
+
+
+
+
+
--- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp
@ -0,0 +1,136 @@
+
+#include "configautotunewidget.h"
+
+#include <QDebug>
+#include <QStringList>
+#include <QtGui/QWidget>
+#include <QtGui/QTextEdit>
+#include <QtGui/QVBoxLayout>
+#include <QtGui/QPushButton>
+#include <QDesktopServices>
+#include <QUrl>
+#include <QList>
+#include "relaytuningsettings.h"
+#include "relaytuning.h"
+#include "stabilizationsettings.h"
+
+ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) :
+    ConfigTaskWidget(parent)
+{
+    m_autotune = new Ui_AutotuneWidget();
+    m_autotune->setupUi(this);
+
+    // Connect automatic signals
+    autoLoadWidgets();
+    disableMouseWheelEvents();
+
+    // Whenever any value changes compute new potential stabilization settings
+    connect(m_autotune->rateTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization()));
+    connect(m_autotune->attitudeTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization()));
+
+    RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager());
+    Q_ASSERT(relayTuning);
+    if(relayTuning)
+        connect(relayTuning, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(recomputeStabilization()));
+
+    // Connect the apply button for the stabilization settings
+    connect(m_autotune->useComputedValues, SIGNAL(pressed()), this, SLOT(saveStabilization()));
+}
+
+/**
+  * Apply the stabilization settings computed
+  */
+void ConfigAutotuneWidget::saveStabilization()
+{
+    StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
+    Q_ASSERT(stabilizationSettings);
+    if(!stabilizationSettings)
+        return;
+
+    // Make sure to recompute in case the other stab settings changed since
+    // the last time
+    recomputeStabilization();
+
+    // Apply this data to the board
+    stabilizationSettings->setData(stabSettings);
+    stabilizationSettings->updated();
+}
+
+/**
+  * Called whenever the gain ratios or measured values
+  * are changed
+  */
+void ConfigAutotuneWidget::recomputeStabilization()
+{
+    RelayTuningSettings *relayTuningSettings = RelayTuningSettings::GetInstance(getObjectManager());
+    Q_ASSERT(relayTuningSettings);
+    if (!relayTuningSettings)
+        return;
+
+    RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager());
+    Q_ASSERT(relayTuning);
+    if(!relayTuning)
+        return;
+
+    StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(getObjectManager());
+    Q_ASSERT(stabilizationSettings);
+    if(!stabilizationSettings)
+        return;
+
+    RelayTuning::DataFields relayTuningData = relayTuning->getData();
+    RelayTuningSettings::DataFields tuningSettingsData = relayTuningSettings->getData();
+    stabSettings = stabilizationSettings->getData();
+
+    // Need to divide these by 100 because that is what the .ui file does
+    // to get the UAVO
+    const double gain_ratio_r = m_autotune->rateTuning->value() / 100.0;
+    const double zero_ratio_r = m_autotune->rateTuning->value() / 100.0;
+    const double gain_ratio_p = m_autotune->attitudeTuning->value() / 100.0;
+    const double zero_ratio_p = m_autotune->attitudeTuning->value() / 100.0;
+
+    // For now just run over roll and pitch
+    for (int i = 0; i < 2; i++) {
+        if (relayTuningData.Period[i] == 0 || relayTuningData.Gain[i] == 0)
+            continue;
+
+        double wu = 1000.0 * 2 * M_PI / relayTuningData.Period[i]; // ultimate freq = output osc freq (rad/s)
+
+        double wc = wu * gain_ratio_r;      // target openloop crossover frequency (rad/s)
+        double zc = wc * zero_ratio_r;      // controller zero location (rad/s)
+        double kpu = 4.0f / M_PI / relayTuningData.Gain[i];  // ultimate gain, i.e. the proportional gain for instablity
+        double kp = kpu * gain_ratio_r;     // proportional gain
+        double ki = zc * kp;                // integral gain
+
+        // Now calculate gains for the next loop out knowing it is the integral of
+        // the inner loop -- the plant is position/velocity = scale*1/s
+        double wc2 = wc * gain_ratio_p;          // crossover of the attitude loop
+        double kp2 = wc2;                       // kp of attitude
+        double ki2 = wc2 * zero_ratio_p * kp2;  // ki of attitude
+
+        switch(i) {
+        case 0: // Roll
+
+            stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP] = kp;
+            stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI] = ki;
+            stabSettings.RollPI[StabilizationSettings::ROLLPI_KP] = kp2;
+            stabSettings.RollPI[StabilizationSettings::ROLLPI_KI] = ki2;
+            break;
+        case 1: // Pitch
+            stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP] = kp;
+            stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI] = ki;
+            stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP] = kp2;
+            stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI] = ki2;
+            break;
+        }
+    }
+
+    // Display these computed settings
+    m_autotune->rollRateKp->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KP]));
+    m_autotune->rollRateKi->setText(QString().number(stabSettings.RollRatePID[StabilizationSettings::ROLLRATEPID_KI]));
+    m_autotune->rollAttitudeKp->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KP]));
+    m_autotune->rollAttitudeKi->setText(QString().number(stabSettings.RollPI[StabilizationSettings::ROLLPI_KI]));
+    m_autotune->pitchRateKp->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KP]));
+    m_autotune->pitchRateKi->setText(QString().number(stabSettings.PitchRatePID[StabilizationSettings::PITCHRATEPID_KI]));
+    m_autotune->pitchAttitudeKp->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP]));
+    m_autotune->pitchAttitudeKi->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI]));
+}
--- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h
@ -0,0 +1,60 @@
+/**
+ ******************************************************************************
+ *
+ * @file       configautotunewidget.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup ConfigPlugin Config Plugin
+ * @{
+ * @brief The Configuration Gadget used to adjust or recalculate autotuning
+ *****************************************************************************/
+/*
+ * 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 CONFIGAUTOTUNE_H
+#define CONFIGAUTOTUNE_H
+
+#include "ui_autotune.h"
+#include "../uavobjectwidgetutils/configtaskwidget.h"
+#include "extensionsystem/pluginmanager.h"
+#include "uavobjectmanager.h"
+#include "uavobject.h"
+#include "stabilizationsettings.h"
+#include "relaytuningsettings.h"
+#include "relaytuning.h"
+#include <QtGui/QWidget>
+#include <QTimer>
+
+class ConfigAutotuneWidget : public ConfigTaskWidget
+{
+    Q_OBJECT
+public:
+    explicit ConfigAutotuneWidget(QWidget *parent = 0);
+
+private:
+    Ui_AutotuneWidget *m_autotune;
+    StabilizationSettings::DataFields stabSettings;
+
+signals:
+
+public slots:
+
+private slots:
+    void recomputeStabilization();
+    void saveStabilization();
+};
+
+#endif // CONFIGAUTOTUNE_H
--- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
@ -32,6 +32,7 @@
 #include "configinputwidget.h"
 #include "configoutputwidget.h"
 #include "configstabilizationwidget.h"
+#include "configautotunewidget.h"
 #include "configcamerastabilizationwidget.h"
 #include "configtxpidwidget.h"
 #include "config_pro_hw_widget.h"
@ -84,6 +85,9 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
    qwd = new ConfigStabilizationWidget(this);
    ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization"));

+    qwd = new ConfigAutotuneWidget(this);
+    ftw->insertTab(ConfigGadgetWidget::autotune, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Autotune"));
+
    qwd = new ConfigCameraStabilizationWidget(this);
    ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab"));

@ -218,8 +222,6 @@ void ConfigGadgetWidget::tabAboutToChange(int i,bool * proceed)
  */
 void ConfigGadgetWidget::updatePipXStatus(UAVObject *)
 {
-    Q_UNUSED(object);
-
    // Restart the disconnection timer.
    pipxTimeout->start(5000);
    if (!pipxConnected)
--- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
@ -48,7 +48,7 @@ class ConfigGadgetWidget: public QWidget
 public:
    ConfigGadgetWidget(QWidget *parent = 0);
    ~ConfigGadgetWidget();
-    enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme};
+    enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme, autotune};

 public slots:
    void onAutopilotConnect();
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@ -71,6 +71,10 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
    $$UAVOBJECT_SYNTHETICS/velocitydesired.h \
    $$UAVOBJECT_SYNTHETICS/velocityactual.h \
    $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \
+    $$UAVOBJECT_SYNTHETICS/guidancesettings.h \
+    $$UAVOBJECT_SYNTHETICS/positiondesired.h \
+    $$UAVOBJECT_SYNTHETICS/relaytuning.h \
+    $$UAVOBJECT_SYNTHETICS/relaytuningsettings.h \
    $$UAVOBJECT_SYNTHETICS/ratedesired.h \
    $$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \
    $$UAVOBJECT_SYNTHETICS/i2cstats.h \
@ -145,6 +149,10 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
    $$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \
    $$UAVOBJECT_SYNTHETICS/velocityactual.cpp \
    $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/positiondesired.cpp \
+    $$UAVOBJECT_SYNTHETICS/relaytuningsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/relaytuning.cpp \
    $$UAVOBJECT_SYNTHETICS/ratedesired.cpp \
    $$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \
    $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \
--- a/shared/uavobjectdefinition/flightstatus.xml
+++ b/shared/uavobjectdefinition/flightstatus.xml
@ -4,7 +4,7 @@
        <field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>

 	<!-- Note these enumerated values should be the same as ManualControlSettings -->
-        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH"/>
+        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH"/>

        <access gcs="readwrite" flight="readwrite"/>
        <telemetrygcs acked="false" updatemode="manual" period="0"/>
--- a/shared/uavobjectdefinition/hwsettings.xml
+++ b/shared/uavobjectdefinition/hwsettings.xml
@ -18,7 +18,7 @@
 		<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
 		<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,Disabled" defaultvalue="Disabled"/>

-		<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,VtolPathFollower,FixedWingPathFollower,Battery,Overo,Radio" options="Disabled,Enabled" defaultvalue="Disabled"/>
+		<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo,Radio" options="Disabled,Enabled" defaultvalue="Disabled"/>
 		<field name="DSMxBind" units=""  type="uint8"  elements="1" defaultvalue="0"/>

 		<access gcs="readwrite" flight="readwrite"/>
--- a/shared/uavobjectdefinition/manualcontrolsettings.xml
+++ b/shared/uavobjectdefinition/manualcontrolsettings.xml
@ -18,13 +18,13 @@
        <field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right" defaultvalue="Always Disarmed"/>

 	<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
-        <field name="Stabilization1Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
-        <field name="Stabilization2Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
-        <field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
+        <field name="Stabilization1Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
+        <field name="Stabilization2Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>
+        <field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude" defaultvalue="Attitude,Attitude,Rate"/>

 	<!-- Note these options values should be identical to those defined in FlightMode -->
        <field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
-        <field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH,Land" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized1,Stabilized2" limits="%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold"/>
+        <field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,Autotune,AltitudeHold,VelocityControl,PositionHold,PathPlanner,RTH,Land" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized1,Stabilized2" limits="%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold,%0401NE:AltitudeHold:VelocityControl:PositionHold;%0402NE:AltitudeHold:VelocityControl:PositionHold"/>

        <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
        <field name="FailsafeBehavior" units="" type="enum" elements="1" options="None,RTH" defaultvalue="None"/>
--- a/shared/uavobjectdefinition/relaytuning.xml
+++ b/shared/uavobjectdefinition/relaytuning.xml
@ -0,0 +1,11 @@
+<xml>
+    <object name="RelayTuning" singleinstance="true" settings="false">
+        <description>The input to the relay tuning.</description>
+	<field name="Period" units="ms" type="float" elementnames="Roll,Pitch,Yaw"/>
+	<field name="Gain" units="(deg/s)/output" type="float" elementnames="Roll,Pitch,Yaw"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>
--- a/shared/uavobjectdefinition/relaytuningsettings.xml
+++ b/shared/uavobjectdefinition/relaytuningsettings.xml
@ -0,0 +1,15 @@
+<xml>
+    <object name="RelayTuningSettings" singleinstance="true" settings="true">
+        <description>Setting to run a relay tuning algorithm</description>
+	<field name="RateGain" units="" type="float" elements="1" defaultvalue="0.3333"/>
+	<field name="AttitudeGain" units="" type="float" elements="1" defaultvalue="0.2"/>
+	<field name="Amplitude" units="" type="float" elements="1" defaultvalue="0.25"/>
+	<field name="HysteresisThresh" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
+	<field name="Mode" units="" type="enum" elements="1" options="Rate,Attitude" defaultvalue="Attitude"/>
+	<field name="Behavior" units="" type="enum" elements="1" options="Measure,Compute,Save" defaultvalue="Compute"/>
+	<access gcs="readwrite" flight="readwrite"/>
+	<telemetrygcs acked="true" updatemode="onchange" period="0"/>
+	<telemetryflight acked="true" updatemode="onchange" period="0"/>
+	<logging updatemode="manual" period="0"/>
+    </object>
+</xml>
--- a/shared/uavobjectdefinition/stabilizationdesired.xml
+++ b/shared/uavobjectdefinition/stabilizationdesired.xml
@ -6,7 +6,7 @@
        <field name="Yaw" units="degrees" type="float" elements="1"/>
        <field name="Throttle" units="%" type="float" elements="1"/>
 	<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings -->
-        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar"/>
+        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"/>
        <access gcs="readwrite" flight="readwrite"/>
        <telemetrygcs acked="false" updatemode="manual" period="0"/>
        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
--- a/shared/uavobjectdefinition/taskinfo.xml
+++ b/shared/uavobjectdefinition/taskinfo.xml
@ -1,9 +1,9 @@
 <xml>
    <object name="TaskInfo" singleinstance="true" settings="false">
        <description>Task information</description>
-	<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher"/>
-	<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher"/>
-	<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher"/>
+	<field name="StackRemaining" units="bytes" type="uint16" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher,Autotune"/>
+	<field name="Running" units="bool" type="enum" options="False,True" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher,Autotune"/>
+	<field name="RunningTime" units="%" type="uint8" elementnames="System,Actuator,Attitude,Sensors,TelemetryTx,TelemetryTxPri,TelemetryRx,GPS,ManualControl,Altitude,Airspeed,Stabilization,AltitudeHold,PathPlanner,PathFollower,FlightPlan,Com2UsbBridge,Usb2ComBridge,OveroSync,ModemRx,ModemTx,ModemStat,EventDispatcher,Autotune"/>
        <access gcs="readwrite" flight="readwrite"/>
        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
        <telemetryflight acked="true" updatemode="periodic" period="10000"/>