From a3f1894cd775afabc6c318f2cb2ea66f0f1563bb Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 24 Aug 2012 21:33:08 -0500 Subject: [PATCH] Autotuning: Make the estimator require the value to toggle above and below the hysteresis level. --- flight/Modules/Stabilization/relay_tuning.c | 18 ++++++++++++++---- .../relaytuningsettings.xml | 1 + 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c index 0f0bb9d55..835786a02 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -53,7 +53,6 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) RelayTuningData relay; RelayTuningGet(&relay); - static bool high = false; static portTickType lastHighTime; static portTickType lastLowTime; @@ -68,6 +67,10 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) 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; @@ -109,8 +112,11 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) accumulated ++; // Make sure 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 */ + 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; @@ -130,9 +136,13 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) lastHighTime = thisTime; high = true; RelayTuningSet(&relay); - } else if ( high && hysteresis && error < 0 ) { /* FALL DETECTED */ + + } else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) { + /* FALLING CROSSING DETECTED */ + lastLowTime = thisTime; high = false; + } return 0; diff --git a/shared/uavobjectdefinition/relaytuningsettings.xml b/shared/uavobjectdefinition/relaytuningsettings.xml index 4ee3cb11b..58718a53d 100644 --- a/shared/uavobjectdefinition/relaytuningsettings.xml +++ b/shared/uavobjectdefinition/relaytuningsettings.xml @@ -4,6 +4,7 @@ +