From 373689207a7dd5a8e39f7e4ca60a53c987a26185 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 21 Jun 2011 19:49:20 -0500 Subject: [PATCH] Add a 1-tap IIR filter into the gyros in the feedback path. This will make the stabilization output a bit more resilient to the high frequency noise from gyros. However this value shouldn't be too high as it will increase the phase delay of the feedback loop and decrease stability. Default is 5 ms. Note: this resests the stabilizationsettings object. Sorry guys. --- flight/Modules/Stabilization/stabilization.c | 20 ++++++++++++++++++- .../stabilizationsettings.xml | 4 +++- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 702020966..eb72391dc 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -77,6 +77,8 @@ static xTaskHandle taskHandle; static StabilizationSettingsData settings; static xQueueHandle queue; float dT = 1; +float gyro_alpha = 0; +float gyro_filtered[3] = {0,0,0}; pid_type pids[PID_MAX]; // Private functions @@ -193,6 +195,11 @@ static void stabilizationTask(void* parameters) local_error[2] = fmod(local_error[2] + 180, 360) - 180; #endif + + for(uint8_t i = 0; i < MAX_AXES; i++) { + gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha); + } + float *attitudeDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; @@ -234,7 +241,7 @@ static void stabilizationTask(void* parameters) case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: { - float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct]-attitudeRaw.gyros[ct]); + float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]); actuatorDesiredAxis[ct] = bound(command); break; } @@ -336,6 +343,17 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) pids[pid].i = *data++; pids[pid].iLim = *data++; } + + // The dT has some jitter iteration to iteration that we don't want to + // make thie result unpredictable. Still, it's nicer to specify the constant + // based on a time (in ms) rather than a fixed multiplier. The error between + // update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this + // calculation + const float fakeDt = 0.0025; + if(settings.GyroTau < 0.0001) + gyro_alpha = 0; // not trusting this to resolve to 0 + else + gyro_alpha = exp(-fakeDt / settings.GyroTau); } diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 991673d14..4eac63d93 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -13,7 +13,9 @@ - + + +