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

Make the system perform an online estimate of the period and amplitude of the

Browse Source 
oscillation during relay tuning
This commit is contained in:
James Cotton 2012-07-29 21:08:46 -05:00
parent 17a0d3ebb4
commit 4ac8df6aa8
2 changed files with 55 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
flight/Modules/Stabilization/stabilization.c
View File

@ -36,6 +36,7 @@
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
#include "actuatordesired.h" #include "actuatordesired.h"
#include "ratedesired.h" #include "ratedesired.h"
#include "relaytuning.h"
#include "relaytuningsettings.h" #include "relaytuningsettings.h"
#include "stabilizationdesired.h" #include "stabilizationdesired.h"
#include "attitudeactual.h" #include "attitudeactual.h"
@ -128,6 +129,8 @@ int32_t StabilizationInitialize()
// Initialize variables // Initialize variables
StabilizationSettingsInitialize(); StabilizationSettingsInitialize();
ActuatorDesiredInitialize(); ActuatorDesiredInitialize();
RelayTuningSettingsInitialize();
RelayTuningInitialize();
#if defined(DIAGNOSTICS) #if defined(DIAGNOSTICS)
RateDesiredInitialize(); RateDesiredInitialize();
#endif #endif
@ -321,12 +324,58 @@ static void stabilizationTask(void* parameters)
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY: case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY:
{ {
RelayTuningSettingsData relay; RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relay); RelayTuningSettingsGet(&relaySettings);
float error = rateDesiredAxis[i] - gyro_filtered[i]; 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); 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; break;
} }
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:

3
shared/uavobjectdefinition/relaytuning.xml
View File

@ -2,7 +2,8 @@
<object name="RelayTuning" singleinstance="true" settings="false"> <object name="RelayTuning" singleinstance="true" settings="false">
<description>The input to the relay tuning.</description> <description>The input to the relay tuning.</description>
<field name="Value" units="deg/s" type="float" elements="1"/> <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"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>