rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1834 - Disable pirouette compensation in self level modes

Browse Source
This commit is contained in:
Rodney Grainger 2015-04-14 21:55:57 +12:00
parent 73d3d3b869
commit 115b206032
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/modules/Stabilization/innerloop.c
View File

@ -241,6 +241,10 @@ static void stabilizationInnerloopTask()
float dT; float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
StabilizationStatusOuterLoopData outerLoop;
StabilizationStatusOuterLoopGet(&outerLoop);
bool allowPiroComp = true;
for (t = 0; t < AXES; t++) { for (t = 0; t < AXES; t++) {
bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]); bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t]; previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
@ -249,6 +253,10 @@ static void stabilizationInnerloopTask()
if (reinit) { if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0; 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]) { switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR: case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings); 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) // attempted piro compensation - rotate pitch and yaw integrals (experimental)
float angleYaw = DEG2RAD(gyro_filtered[2] * dT); float angleYaw = DEG2RAD(gyro_filtered[2] * dT);
float sinYaw = sinf(angleYaw); float sinYaw = sinf(angleYaw);