Rev 1992 | Rev 2017 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1992 | Rev 2015 | ||
---|---|---|---|
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 PPArt[2], DPart[2], PPartYaw, DPartYaw; |
156 | int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */; |
157 | static int32_t IPart[2] = { 0, 0 }; |
157 | static int32_t IPart[2] = { 0, 0 }; |
158 | static uint16_t emergencyFlightTime; |
158 | static uint16_t emergencyFlightTime; |
Line 159... | Line 159... | ||
159 | static int8_t debugDataTimer = 1; |
159 | static int8_t debugDataTimer = 1; |
Line 283... | Line 283... | ||
283 | /* Calculate control feedback from angle (gyro integral) */ |
283 | /* Calculate control feedback from angle (gyro integral) */ |
284 | /* and angular velocity (gyro signal) */ |
284 | /* and angular velocity (gyro signal) */ |
285 | /************************************************************************/ |
285 | /************************************************************************/ |
286 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
286 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
287 | for (axis = PITCH; axis <= ROLL; axis++) { |
287 | for (axis = PITCH; axis <= ROLL; axis++) { |
288 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
288 | PDPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
Line 289... | Line 289... | ||
289 | 289 | ||
290 | /* |
290 | /* |
291 | * 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. |
292 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
292 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
293 | * where pfactor is in [0..1]. |
293 | * where pfactor is in [0..1]. |
294 | */ |
294 | */ |
295 | DPart[axis] = (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
295 | PDPart[axis] += (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
296 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
296 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
Line 297... | Line 297... | ||
297 | } |
297 | } |
298 | 298 | ||
Line 299... | Line 299... | ||
299 | PPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
299 | PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
300 | DPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
300 | PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
Line 301... | Line 301... | ||
301 | 301 | ||
Line 365... | Line 365... | ||
365 | */ |
365 | */ |
366 | if (gyroIFactor) { |
366 | if (gyroIFactor) { |
367 | // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
367 | // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
368 | // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
368 | // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
369 | // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
369 | // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
370 | IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
370 | IPart[axis] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
371 | } else { |
371 | } else { |
372 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
372 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
373 | // To keep up with a full stick PDPart should be about 156... |
373 | // To keep up with a full stick PDPart should be about 156... |
374 | IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
374 | IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
375 | } |
375 | } |