Rev 1991 | Rev 2015 | Go to most recent revision | Show entire file | Regard 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); |