Rev 1991 | Rev 2015 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1991 | Rev 1992 | ||
|---|---|---|---|
| Line 151... | Line 151... | ||
| 151 | int16_t tmp_int; |
151 | int16_t tmp_int; |
| 152 | // Mixer Fractions that are combined for Motor Control |
152 | // Mixer Fractions that are combined for Motor Control |
| 153 | int16_t yawTerm, throttleTerm, term[2]; |
153 | int16_t yawTerm, throttleTerm, term[2]; |
| Line 154... | Line 154... | ||
| 154 | 154 | ||
| 155 | // PID controller variables |
155 | // PID controller variables |
| 156 | int16_t PDPart[2], PDPartYaw, PPart[2]; |
156 | int16_t PPArt[2], DPart[2], PPartYaw, DPartYaw; |
| 157 | static int32_t IPart[2] = { 0, 0 }; |
- | |
| 158 | // static int32_t yawControlRate = 0; |
- | |
| 159 | - | ||
| 160 | // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
- | |
| 161 | // static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0; |
- | |
| 162 | // static int32_t CorrectionPitch, CorrectionRoll; |
- | |
| 163 | 157 | static int32_t IPart[2] = { 0, 0 }; |
|
| 164 | static uint16_t emergencyFlightTime; |
158 | static uint16_t emergencyFlightTime; |
| Line 165... | Line 159... | ||
| 165 | static int8_t debugDataTimer = 1; |
159 | static int8_t debugDataTimer = 1; |
| 166 | 160 | ||
| Line 288... | Line 282... | ||
| 288 | 282 | ||
| 289 | /* Calculate control feedback from angle (gyro integral) */ |
283 | /* Calculate control feedback from angle (gyro integral) */ |
| 290 | /* and angular velocity (gyro signal) */ |
284 | /* and angular velocity (gyro signal) */ |
| 291 | /************************************************************************/ |
285 | /************************************************************************/ |
| 292 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
- | |
| 293 | 286 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
|
| 294 | for (axis = PITCH; axis <= ROLL; axis++) { |
287 | for (axis = PITCH; axis <= ROLL; axis++) { |
| Line 295... | Line 288... | ||
| 295 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
288 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| 296 | 289 | ||
| 297 | /* |
290 | /* |
| 298 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
291 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
| 299 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
292 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
| 300 | * where pfactor is in [0..1]. |
293 | * where pfactor is in [0..1]. |
| 301 | */ |
- | |
| 302 | PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
- | |
| 303 | * (int16_t) dynamicParams.gyroD) / 16; |
294 | */ |
| 304 | 295 | DPart[axis] = (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
|
| Line 305... | Line 296... | ||
| 305 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
296 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| 306 | } |
297 | } |
| 307 | - | ||
| Line 308... | Line 298... | ||
| 308 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L |
298 | |
| 309 | / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 |
299 | PPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| Line 310... | Line 300... | ||
| 310 | / CONTROL_SCALING)); |
300 | DPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
| 311 | 301 | ||
| 312 | // limit control feedback |
302 | // limit control feedback |
| 313 | CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
303 | // CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |