Rev 2048 | Rev 2051 | Go to most recent revision | Show entire file | Regard 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 |