mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
Autotuning: Make the estimator require the value to toggle above and below the hysteresis level.
This commit is contained in:
parent
8babe5d8a4
commit
a3f1894cd7
@ -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;
|
||||
|
@ -4,6 +4,7 @@
|
||||
<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.15"/>
|
||||
<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"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user