Rev 2129 | Rev 2132 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2129 | Rev 2131 | ||
|---|---|---|---|
| Line 128... | Line 128... | ||
| 128 | target[axis] -= OVER360; |
128 | target[axis] -= OVER360; |
| 129 | } else if (target[axis] <= -OVER180) { |
129 | } else if (target[axis] <= -OVER180) { |
| 130 | target[axis] += OVER360; |
130 | target[axis] += OVER360; |
| 131 | } |
131 | } |
| Line 132... | Line 132... | ||
| 132 | 132 | ||
| Line -... | Line 133... | ||
| - | 133 | error[axis] = attitude[axis] - target[axis]; |
|
| - | 134 | ||
| - | 135 | #define ROTATETARGET 1 |
|
| - | 136 | // #define TRUNCATEERROR 1 |
|
| - | 137 | ||
| 133 | /* This is the difference limitation only way. The 2 subtrahends stay unmodified. */ |
138 | #ifdef(ROTATETARGET) |
| - | 139 | if(abs(error[axis]) > OVER180) { |
|
| - | 140 | // The shortest way from attitude to target crosses -180. |
|
| 134 | 141 | // 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. |
|
| - | 142 | // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
|
| - | 143 | if (error[axis] > 0) { |
|
| - | 144 | if (error[axis] < OVER360 - maxError[axis]) { |
|
| - | 145 | // too much err. |
|
| - | 146 | error[axis] = -maxError[axis]; |
|
| - | 147 | target[axis] = attitude[axis] + maxError[axis]; |
|
| - | 148 | if (target[axis]) > OVER180) target[axis] -= OVER360; |
|
| 135 | error[axis] = attitude[axis] - target[axis]; |
149 | } else { |
| - | 150 | // Normal case, we just need to correct for the wrap. Error will be negative. |
|
| - | 151 | error[axis] -= OVER360; |
|
| 136 | if (error[axis] > OVER180) { |
152 | } |
| - | 153 | } else { |
|
| - | 154 | if (error[axis] > maxError[axis] - OVER360) { |
|
| - | 155 | // too much err. |
|
| - | 156 | error[axis] = maxError[axis]; |
|
| - | 157 | target[axis] = attitude[axis] - maxError[axis]; |
|
| - | 158 | if (target[axis]) < -OVER180) target[axis] += OVER360; |
|
| 137 | error[axis] -= OVER360; |
159 | } else { |
| - | 160 | // Normal case, we just need to correct for the wrap. Error will be negative. |
|
| - | 161 | error[axis] += OVER360; |
|
| - | 162 | } |
|
| - | 163 | } |
|
| - | 164 | } else { |
|
| - | 165 | // Simple case, linear range. |
|
| - | 166 | if (error[axis] > maxError[axis]) { |
|
| - | 167 | error[axis] = maxError[axis]; |
|
| - | 168 | target[axis] = attitude[axis] - maxError[axis]; |
|
| - | 169 | } else if (error[axis] < -maxError[axis]) { |
|
| - | 170 | error[axis] = -maxError[axis]; |
|
| 138 | } else if (error[axis] <= -OVER180) { |
171 | target[axis] = attitude[axis] + maxError[axis]; |
| - | 172 | } |
|
| 139 | error[axis] += OVER360; |
173 | } |
| 140 | } |
174 | #endif |
| 141 | // Believe it or not, the below limiter does NOT solve the wrapping problem. Must do explicitly. |
175 | #ifdef(TUNCATEERROR) |
| 142 | if (error[axis] > maxError[axis]) { |
176 | if (error[axis] > maxError[axis]) { |
| 143 | error[axis] = maxError[axis]; |
177 | error[axis] = maxError[axis]; |
| 144 | } else if (error[axis] < -maxError[axis]) { |
178 | } else if (error[axis] < -maxError[axis]) { |
| 145 | error[axis] = -maxError[axis]; |
179 | error[axis] = -maxError[axis]; |
| 146 | } else { |
180 | } else { |
| - | 181 | // update I parts here for angles mode. I parts in rate mode is something different. |
|
| Line 147... | Line 182... | ||
| 147 | // update I parts here for angles mode. I parts in rate mode is something different. |
182 | } |
| 148 | } |
183 | #endif |
| 149 | 184 | ||
| 150 | /* |
185 | /* |