rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Autotuning: Make the estimator require the value to toggle above and below the hysteresis level.

Browse Source
This commit is contained in:
James Cotton 2012-08-24 21:33:08 -05:00
parent 8babe5d8a4
commit a3f1894cd7
2 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/Modules/Stabilization/relay_tuning.c
View File

@ -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;

1
shared/uavobjectdefinition/relaytuningsettings.xml
View File

@ -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"/>