From 115b2060329c5a194a2100f016516e339f6d0d45 Mon Sep 17 00:00:00 2001 From: Rodney Grainger Date: Tue, 14 Apr 2015 21:55:57 +1200 Subject: [PATCH 1/2] OP-1834 - Disable pirouette compensation in self level modes --- flight/modules/Stabilization/innerloop.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 2ed1ae57b..a395ca38c 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -241,6 +241,10 @@ static void stabilizationInnerloopTask() float dT; dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + StabilizationStatusOuterLoopData outerLoop; + StabilizationStatusOuterLoopGet(&outerLoop); + bool allowPiroComp = true; + for (t = 0; t < AXES; t++) { bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]); previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t]; @@ -249,6 +253,10 @@ static void stabilizationInnerloopTask() if (reinit) { stabSettings.innerPids[t].iAccumulator = 0; } + // Any self leveling on roll or pitch must prevent pirouette compenstation + if(t < STABILIZATIONSTATUS_INNERLOOP_YAW && StabilizationStatusOuterLoopToArray(outerLoop)[t] !=STABILIZATIONSTATUS_OUTERLOOP_DIRECT) { + allowPiroComp = false; + } switch (StabilizationStatusInnerLoopToArray(enabled)[t]) { case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR: stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings); @@ -322,7 +330,7 @@ static void stabilizationInnerloopTask() } } - if (stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) { + if (allowPiroComp && 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 = DEG2RAD(gyro_filtered[2] * dT); float sinYaw = sinf(angleYaw); From aa9ed6a62a69ecb4d1157bbc5b95cf97e40aec77 Mon Sep 17 00:00:00 2001 From: Rodney Grainger Date: Tue, 14 Apr 2015 22:00:11 +1200 Subject: [PATCH 2/2] OP-1843 - uncrustify --- flight/modules/Stabilization/innerloop.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index a395ca38c..fbcad210a 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -243,7 +243,7 @@ static void stabilizationInnerloopTask() StabilizationStatusOuterLoopData outerLoop; StabilizationStatusOuterLoopGet(&outerLoop); - bool allowPiroComp = true; + bool allowPiroComp = true; for (t = 0; t < AXES; t++) { bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]); @@ -253,9 +253,9 @@ static void stabilizationInnerloopTask() if (reinit) { stabSettings.innerPids[t].iAccumulator = 0; } - // Any self leveling on roll or pitch must prevent pirouette compenstation - if(t < STABILIZATIONSTATUS_INNERLOOP_YAW && StabilizationStatusOuterLoopToArray(outerLoop)[t] !=STABILIZATIONSTATUS_OUTERLOOP_DIRECT) { - allowPiroComp = false; + // Any self leveling on roll or pitch must prevent pirouette compensation + if (t < STABILIZATIONSTATUS_INNERLOOP_YAW && StabilizationStatusOuterLoopToArray(outerLoop)[t] != STABILIZATIONSTATUS_OUTERLOOP_DIRECT) { + allowPiroComp = false; } switch (StabilizationStatusInnerLoopToArray(enabled)[t]) { case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR: