| 130,15 → 130,49 |
|---|
| target[axis] += OVER360; |
| } |
| |
| /* This is the difference limitation only way. The 2 subtrahends stay unmodified. */ |
| error[axis] = attitude[axis] - target[axis]; |
| |
| error[axis] = attitude[axis] - target[axis]; |
| if (error[axis] > OVER180) { |
| error[axis] -= OVER360; |
| } else if (error[axis] <= -OVER180) { |
| error[axis] += OVER360; |
| #define ROTATETARGET 1 |
| // #define TRUNCATEERROR 1 |
| |
| #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. |
| // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
| if (error[axis] > 0) { |
| if (error[axis] < OVER360 - maxError[axis]) { |
| // too much err. |
| error[axis] = -maxError[axis]; |
| target[axis] = attitude[axis] + maxError[axis]; |
| 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; |
| } |
| } else { |
| if (error[axis] > maxError[axis] - OVER360) { |
| // too much err. |
| error[axis] = maxError[axis]; |
| target[axis] = attitude[axis] - maxError[axis]; |
| 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; |
| } |
| } |
| } else { |
| // Simple case, linear range. |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| target[axis] = attitude[axis] - maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| error[axis] = -maxError[axis]; |
| target[axis] = attitude[axis] + maxError[axis]; |
| } |
| } |
| // Believe it or not, the below limiter does NOT solve the wrapping problem. Must do explicitly. |
| #endif |
| #ifdef(TUNCATEERROR) |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| 146,6 → 180,7 |
|---|
| } else { |
| // update I parts here for angles mode. I parts in rate mode is something different. |
| } |
| #endif |
| |
| /* |
| * This is the beginning of a version that adjusts the target to differ from the attitude |