| 130,19 → 130,37 |
|---|
| target[axis] += OVER360; |
| } |
| |
| //if (reverse[axis]) |
| /* This is the difference limitation only way. The 2 subtrahends stay unmodified. */ |
| |
| error[axis] = attitude[axis] - target[axis]; |
| // else |
| // error[axis] = attitude[axis] - target[axis]; |
| |
| if (error[axis] > OVER180) { |
| error[axis] -= OVER360; |
| } else if (error[axis] <= -OVER180) { |
| error[axis] += OVER360; |
| } |
| // Believe it or not, the below limiter does NOT solve the wrapping problem. Must do explicitly. |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| error[axis] =- maxError[axis]; |
| error[axis] = -maxError[axis]; |
| } else { |
| // update I parts here for angles mode. Ĩ parts in rate mode is something different. |
| // update I parts here for angles mode. I parts in rate mode is something different. |
| } |
| |
| /* |
| * 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) */ |