130,22 → 130,57 |
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) { |
#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] <= -OVER180) { |
} |
} 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; |
} |
// Believe it or not, the below limiter does NOT solve the wrapping problem. Must do explicitly. |
} |
} 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]; |
} |
} |
#endif |
#ifdef(TUNCATEERROR) |
if (error[axis] > maxError[axis]) { |
error[axis] = maxError[axis]; |
} else if (error[axis] < -maxError[axis]) { |
error[axis] = -maxError[axis]; |
} 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 |