rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +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; RelayTuningData relay;
RelayTuningGet(&relay); RelayTuningGet(&relay);
static bool high = false;
static portTickType lastHighTime; static portTickType lastHighTime;
static portTickType lastLowTime; static portTickType lastLowTime;
@ -68,6 +67,10 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
static bool rateRelayRunning[MAX_AXES]; 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 // On first run initialize estimates to something reasonable
if(reinit) { if(reinit) {
rateRelayRunning[axis] = false; rateRelayRunning[axis] = false;
@ -109,8 +112,11 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
accumulated ++; accumulated ++;
// Make sure we've had enough time since last transition then check for a change in the output // 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; bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */
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_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated;
float this_gain = this_amplitude / relaySettings.Amplitude; 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; lastHighTime = thisTime;
high = true; high = true;
RelayTuningSet(&relay); RelayTuningSet(&relay);
} else if ( high && hysteresis && error < 0 ) { /* FALL DETECTED */
} else if ( high && time_hysteresis && error < -relaySettings.HysteresisThresh ) {
/* FALLING CROSSING DETECTED */
lastLowTime = thisTime; lastLowTime = thisTime;
high = false; high = false;
} }
return 0; 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="RateGain" units="" type="float" elements="1" defaultvalue="0.3333"/>
<field name="AttitudeGain" units="" type="float" elements="1" defaultvalue="0.2"/> <field name="AttitudeGain" units="" type="float" elements="1" defaultvalue="0.2"/>
<field name="Amplitude" units="" type="float" elements="1" defaultvalue="0.15"/> <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="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"/> <field name="Behavior" units="" type="enum" elements="1" options="Measure,Compute,Save" defaultvalue="Compute"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>