From 4ac8df6aa8ca194f31263ef9c6d0a634dfa5c55c Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Sun, 29 Jul 2012 21:08:46 -0500
Subject: [PATCH] Make the system perform an online estimate of the period and
 amplitude of the oscillation during relay tuning

---
 flight/Modules/Stabilization/stabilization.c | 57 ++++++++++++++++++--
 shared/uavobjectdefinition/relaytuning.xml   |  3 +-
 2 files changed, 55 insertions(+), 5 deletions(-)

diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c
index c49f5c103..3eb2c9c05 100644
--- a/flight/Modules/Stabilization/stabilization.c
+++ b/flight/Modules/Stabilization/stabilization.c
@@ -36,6 +36,7 @@
 #include "stabilizationsettings.h"
 #include "actuatordesired.h"
 #include "ratedesired.h"
+#include "relaytuning.h"
 #include "relaytuningsettings.h"
 #include "stabilizationdesired.h"
 #include "attitudeactual.h"
@@ -128,6 +129,8 @@ int32_t StabilizationInitialize()
 	// Initialize variables
 	StabilizationSettingsInitialize();
 	ActuatorDesiredInitialize();
+	RelayTuningSettingsInitialize();
+	RelayTuningInitialize();
 #if defined(DIAGNOSTICS)
 	RateDesiredInitialize();
 #endif
@@ -321,12 +324,58 @@ static void stabilizationTask(void* parameters)
 			{
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY:
 				{
-					RelayTuningSettingsData relay;
-					RelayTuningSettingsGet(&relay);
+					RelayTuningSettingsData relaySettings;
+					RelayTuningSettingsGet(&relaySettings);
 					float error = rateDesiredAxis[i] - gyro_filtered[i];
-					float command = error > 0 ? relay.Amplitude : -relay.Amplitude;
+					float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude;
 					actuatorDesiredAxis[i] = bound(command);
-					break;
+
+					RelayTuningData relay;
+					RelayTuningGet(&relay);
+
+					static bool high = false;
+					static portTickType lastHighTime;
+					static portTickType lastLowTime;
+					portTickType thisTime = xTaskGetTickCount();
+
+					static float accum_cos, accum_sin;
+					static uint32_t accumulated = 0;
+
+					const uint16_t DEGLITCH_TIME = 20; // ms
+					const float AMPLITUDE_ALPHA = 0.95;
+					const float PERIOD_ALPHA = 0.95;
+
+					// Make sure the period can't go below limit
+					if (relay.Period[i] < DEGLITCH_TIME)
+						relay.Period[i] = DEGLITCH_TIME;
+
+					// Project the error onto a sine and cosine of the same frequency
+					// to accumulate the average amplitude
+					float dT = thisTime - lastHighTime;
+					float phase = dT * 2 * M_PI / relay.Period[i];
+					accum_cos += cosf(phase) * error;
+					accum_sin += sinf(phase) * error;
+					accumulated ++;
+
+					// Make susre we've had enough time since last transition then check for a change in the output
+					bool hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
+					if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */
+						float this_amplitude = 2 * sqrtf(accum_cos*accum_cos + accum_sin*accum_sin) / accumulated;
+						accumulated = 0;
+						accum_cos = 0;
+						accum_sin = 0;
+
+						// Low pass filter each amplitude and period
+						relay.Amplitude[i] = relay.Amplitude[i] * AMPLITUDE_ALPHA + this_amplitude * (1 - AMPLITUDE_ALPHA);
+						relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
+						lastHighTime = thisTime;
+						high = true;
+						RelayTuningSet(&relay);
+					} else if ( high && hysteresis && error < 0 ) { /* FALL DETECTED */
+						lastLowTime = thisTime;
+						high = false;
+					}
+
 					break;
 				}
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
diff --git a/shared/uavobjectdefinition/relaytuning.xml b/shared/uavobjectdefinition/relaytuning.xml
index f13bfc9f2..ef54f8b7f 100644
--- a/shared/uavobjectdefinition/relaytuning.xml
+++ b/shared/uavobjectdefinition/relaytuning.xml
@@ -2,7 +2,8 @@
     <object name="RelayTuning" singleinstance="true" settings="false">
         <description>The input to the relay tuning.</description>
 	<field name="Value" units="deg/s" type="float" elements="1"/>
-	<field name="Axis" units="" type="enum" elements="1" options="None,Roll,Pitch" defaultvalue="None"/>
+	<field name="Period" units="ms" type="float" elementnames="Roll,Pitch,Yaw"/>
+	<field name="Amplitude" units="deg/s" 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"/>