Rev 1986 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1986 | Rev 1988 | ||
|---|---|---|---|
| Line 153... | Line 153... | ||
| 153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| 154 | * it is hardly worth the trouble. |
154 | * it is hardly worth the trouble. |
| 155 | ************************************************************************/ |
155 | ************************************************************************/ |
| Line 156... | Line 156... | ||
| 156 | 156 | ||
| - | 157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
|
| 157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
158 | int16_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256; |
| 158 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
| Line 159... | Line 160... | ||
| 159 | } |
160 | } |
| 160 | 161 | ||
| 161 | void setStaticAttitudeAngles(void) { |
162 | void setStaticAttitudeAngles(void) { |
| Line 293... | Line 294... | ||
| 293 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
294 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
| 294 | // are less than ....., or reintroduce Kalman. |
295 | // are less than ....., or reintroduce Kalman. |
| 295 | // Well actually the Z axis acc. check is not so silly. |
296 | // Well actually the Z axis acc. check is not so silly. |
| 296 | uint8_t axis; |
297 | uint8_t axis; |
| 297 | int32_t temp; |
298 | int32_t temp; |
| - | 299 | ||
| - | 300 | uint8_t ca = controlActivity >> 8; |
|
| - | 301 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
|
| - | 302 | ||
| - | 303 | if (highControlActivity) { |
|
| 298 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
304 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| - | 305 | } else { |
|
| 299 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
306 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| - | 307 | } |
|
| Line 300... | Line 308... | ||
| 300 | 308 | ||
| 301 | if (accVector <= dynamicParams.maxAccVector) { |
309 | if (accVector <= dynamicParams.maxAccVector) { |
| Line 302... | Line 310... | ||
| 302 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
310 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| Line 312... | Line 320... | ||
| 312 | 320 | ||
| 313 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
321 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
| 314 | permilleAcc /= 2; |
322 | permilleAcc /= 2; |
| 315 | debugFullWeight = 0; |
323 | debugFullWeight = 0; |
| 316 | */ |
- | |
| Line 317... | Line 324... | ||
| 317 | uint8_t ca = controlActivity >> 8; |
324 | */ |
| 318 | 325 | ||
| 319 | if (ca > staticParams.maxControlActivity) { // reduce effect during stick control activity |
- | |
| 320 | permilleAcc /= 4; |
326 | if (highControlActivity) { // reduce effect during stick control activity |
| 321 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
327 | permilleAcc /= 4; |
| 322 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
- | |
| 323 | permilleAcc /= 4; |
328 | if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
| - | 329 | permilleAcc /= 4; |
|
| - | 330 | } |
|
| 324 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
331 | } else { |
| Line 325... | Line 332... | ||
| 325 | } |
332 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| 326 | } |
333 | } |
| 327 | 334 | ||