| 135,7 → 135,7 |
|---|
| #define ROTATETARGET 1 |
| // #define TRUNCATEERROR 1 |
| |
| #ifdef(ROTATETARGET) |
| #ifdef ROTATETARGET |
| if(abs(error[axis]) > OVER180) { |
| // The shortest way from attitude to target crosses -180. |
| // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative. |
| 145,7 → 145,7 |
|---|
| // too much err. |
| error[axis] = -maxError[axis]; |
| target[axis] = attitude[axis] + maxError[axis]; |
| if (target[axis]) > OVER180) target[axis] -= OVER360; |
| if (target[axis] > OVER180) target[axis] -= OVER360; |
| } else { |
| // Normal case, we just need to correct for the wrap. Error will be negative. |
| error[axis] -= OVER360; |
| 155,7 → 155,7 |
|---|
| // too much err. |
| error[axis] = maxError[axis]; |
| target[axis] = attitude[axis] - maxError[axis]; |
| if (target[axis]) < -OVER180) target[axis] += OVER360; |
| if (target[axis] < -OVER180) target[axis] += OVER360; |
| } else { |
| // Normal case, we just need to correct for the wrap. Error will be negative. |
| error[axis] += OVER360; |
| 172,7 → 172,7 |
|---|
| } |
| } |
| #endif |
| #ifdef(TUNCATEERROR) |
| #ifdef TUNCATEERROR |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| 182,20 → 182,6 |
|---|
| } |
| #endif |
| |
| /* |
| * This is the beginning of a version that adjusts the target to differ from the attitude |
| * by a limited amount. This will eliminate memory over large errors but also knock target angles. |
| * Idea: The limit could be calculated from the max. servo deflection divided by I factor... |
| * |
| */ |
| /* |
| if(abs(attitude[axis]-target[axis]) > OVER180) { |
| if(target[axis] > attitude[axis]) { |
| |
| } |
| } |
| */ |
| |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| 211,10 → 197,7 |
|---|
| |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| // if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| // else |
| // PDPart[axis] -= anglePart; |
| } |
| |
| // Add I parts here... these are integrated errors. |