From dcb354b0b1652870e3c4759b9db546092b19bfe8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 2 Oct 2014 22:42:08 +0200 Subject: [PATCH] OP-1516 Enable PiroComp per setting --- flight/modules/Stabilization/innerloop.c | 6 ++---- shared/uavobjectdefinition/stabilizationbank.xml | 2 ++ shared/uavobjectdefinition/stabilizationsettingsbank1.xml | 2 ++ shared/uavobjectdefinition/stabilizationsettingsbank2.xml | 2 ++ shared/uavobjectdefinition/stabilizationsettingsbank3.xml | 2 ++ 5 files changed, 10 insertions(+), 4 deletions(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 557ba65f2..1314bfdd8 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -239,7 +239,6 @@ static void stabilizationInnerloopTask() float *rate = &rateDesired.Roll; float *actuatorDesiredAxis = &actuator.Roll; int t; - bool needPiroComp = false; float dT; dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); @@ -295,10 +294,9 @@ static void stabilizationInnerloopTask() ); pid_scaler ascaler = create_pid_scaler(t); float factor = 1.0f - fabsf(stickinput[t]); - ascaler.i *= factor; // this prevents Integral from getting too high while controlled manually + ascaler.i *= factor; // this prevents Integral from getting too high while controlled manually float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT); actuatorDesiredAxis[t] = (1.0f - factor * stickinput[t]) + factor * arate; - needPiroComp = true; } break; case STABILIZATIONSTATUS_INNERLOOP_DIRECT: @@ -332,7 +330,7 @@ static void stabilizationInnerloopTask() } } - if (needPiroComp && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) { + if (stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) { // attempted piro compensation - rotate pitch and yaw integrals (experimental) float angleYaw = gyro_filtered[2] * dT; float sinYaw = sin_lookup_deg(angleYaw); diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml index b1fbe5c13..4b44de94f 100644 --- a/shared/uavobjectdefinition/stabilizationbank.xml +++ b/shared/uavobjectdefinition/stabilizationbank.xml @@ -15,6 +15,8 @@ + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml index f3c1d654b..442c2aba0 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml @@ -15,6 +15,8 @@ + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml index 09d60c57c..8053d0353 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml @@ -15,6 +15,8 @@ + + diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml index 787819fb2..cda67ef3e 100644 --- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml +++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml @@ -15,6 +15,8 @@ + +