Rev 2086 | Rev 2098 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2086 | Rev 2092 | ||
|---|---|---|---|
| Line 195... | Line 195... | ||
| 195 | PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4); |
195 | PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4); |
| 196 | // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick. |
196 | // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick. |
| 197 | // In HH mode, the I part is summed from P and D of gyros minus stick. |
197 | // In HH mode, the I part is summed from P and D of gyros minus stick. |
| 198 | if (gyroIFactor) { |
198 | if (gyroIFactor) { |
| 199 | int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); |
199 | int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); |
| 200 | if (axis == 0) debugOut.analog[28] = iDiff; |
200 | //if (axis == 0) debugOut.analog[28] = iDiff; |
| 201 | PDPart += iDiff; |
201 | PDPart += iDiff; |
| 202 | IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
202 | IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| 203 | } else { |
203 | } else { |
| 204 | IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
204 | IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| 205 | } |
205 | } |