rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-1516 fixed obvious issues

Browse Source
This commit is contained in:
Corvus Corax 2014-10-02 23:38:06 +02:00
parent dcb354b0b1
commit a12e2708a2
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -296,7 +296,7 @@ static void stabilizationInnerloopTask()
float factor = 1.0f - fabsf(stickinput[t]);
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;
actuatorDesiredAxis[t] = (1.0f - factor) * stickinput[t] + factor * arate;
}
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
@ -332,13 +332,13 @@ static void stabilizationInnerloopTask()
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);
float cosYaw = cos_lookup_deg(angleYaw);
float angleYaw = DEG2RAD(gyro_filtered[2] * dT);
float sinYaw = sinf(angleYaw);
float cosYaw = cosf(angleYaw);
float rollAcc = stabSettings.innerPids[0].iAccumulator / stabSettings.innerPids[0].iLim;
float pitchAcc = stabSettings.innerPids[1].iAccumulator / stabSettings.innerPids[1].iLim;
stabSettings.innerPids[0].iAccumulator = stabSettings.innerPids[0].iLim * (cosYaw * rollAcc - sinYaw * pitchAcc);
stabSettings.innerPids[1].iAccumulator = stabSettings.innerPids[1].iLim * (cosYaw * pitchAcc + sinYaw * rollAcc);
stabSettings.innerPids[0].iAccumulator = stabSettings.innerPids[0].iLim * (cosYaw * rollAcc + sinYaw * pitchAcc);
stabSettings.innerPids[1].iAccumulator = stabSettings.innerPids[1].iLim * (cosYaw * pitchAcc - sinYaw * rollAcc);
}
{