Rev 2048 | Rev 2051 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2048 | Rev 2049 | ||
|---|---|---|---|
| Line 261... | Line 261... | ||
| 261 | /* Calculate control feedback from angle (gyro integral) */ |
261 | /* Calculate control feedback from angle (gyro integral) */ |
| 262 | /* and angular velocity (gyro signal) */ |
262 | /* and angular velocity (gyro signal) */ |
| 263 | /************************************************************************/ |
263 | /************************************************************************/ |
| 264 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
264 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
| 265 | for (axis = PITCH; axis <= ROLL; axis++) { |
265 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 266 | PDPart[axis] = attitude[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
266 | PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
| 267 | PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)); |
267 | PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
| 268 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
268 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| 269 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
269 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| 270 | } |
270 | } |
| Line 271... | Line 271... | ||
| 271 | 271 | ||
| Line 275... | Line 275... | ||
| 275 | 275 | ||
| 276 | // TODO: Not quite right: We want to limit targetHeading to be max. 50000 from heading. This is the wrong var. fixed. |
276 | // TODO: Not quite right: We want to limit targetHeading to be max. 50000 from heading. This is the wrong var. fixed. |
| 277 | CHECK_MIN_MAX(headingDiff, -50000, 50000); |
277 | CHECK_MIN_MAX(headingDiff, -50000, 50000); |
| Line 278... | Line 278... | ||
| 278 | debugOut.analog[29] = headingDiff; |
278 | debugOut.analog[29] = headingDiff; |
| 279 | 279 | ||
| Line 280... | Line 280... | ||
| 280 | PDPartYaw = (int32_t) (headingDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
280 | PDPartYaw = (int32_t)(headingDiff * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
| 281 | PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
281 | PDPartYaw += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_PITCHROLL >> 6); |
| Line 282... | Line 282... | ||
| 282 | 282 | ||