Rev 2065 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2065 | Rev 2084 | ||
|---|---|---|---|
| Line 280... | Line 280... | ||
| 280 | // are less than ....., or reintroduce Kalman. |
280 | // are less than ....., or reintroduce Kalman. |
| 281 | // Well actually the Z axis acc. check is not so silly. |
281 | // Well actually the Z axis acc. check is not so silly. |
| 282 | uint8_t axis; |
282 | uint8_t axis; |
| 283 | int32_t temp; |
283 | int32_t temp; |
| Line -... | Line 284... | ||
| - | 284 | ||
| - | 285 | // for debug LEDs, to be removed with that. |
|
| - | 286 | static uint8_t controlActivityFlash=1; |
|
| - | 287 | static uint8_t accFlash=1; |
|
| - | 288 | #define CF_MAX 10 |
|
| - | 289 | // [1..n[=off [n..10]=on |
|
| - | 290 | // 1 -->1=on, 2=on, ..., 10=on |
|
| - | 291 | // 2 -->1=off,2=on, ..., 10=on |
|
| - | 292 | // 10-->1=off,2=off,..., 10=on |
|
| 284 | 293 | // 11-->1=off,2=off,..., 10=off |
|
| 285 | uint16_t ca = controlActivity >> 6; |
294 | uint16_t ca = controlActivity >> 6; |
| 286 | uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
295 | uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
| 287 | if (!controlActivityWeighted) controlActivityWeighted = 1; |
296 | if (!controlActivityWeighted) controlActivityWeighted = 1; |
| 288 | uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
297 | uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
| Line 292... | Line 301... | ||
| 292 | int32_t accDerived; |
301 | int32_t accDerived; |
| Line 293... | Line 302... | ||
| 293 | 302 | ||
| 294 | debugOut.analog[14] = controlActivity; |
303 | debugOut.analog[14] = controlActivity; |
| Line 295... | Line -... | ||
| 295 | debugOut.analog[15] = accVector; |
- | |
| 296 | - | ||
| 297 | /* |
- | |
| 298 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
- | |
| 299 | permilleAcc /= 2; |
- | |
| 300 | debugFullWeight = 0; |
- | |
| 301 | } |
- | |
| 302 | - | ||
| 303 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
- | |
| 304 | permilleAcc /= 2; |
- | |
| 305 | debugFullWeight = 0; |
- | |
| 306 | */ |
304 | debugOut.analog[15] = accVector; |
| 307 | 305 | ||
| 308 | debugOut.analog[20] = controlActivityWeighted; |
306 | debugOut.analog[20] = controlActivityWeighted; |
| Line 309... | Line 307... | ||
| 309 | debugOut.analog[21] = accVectorWeighted; |
307 | debugOut.analog[21] = accVectorWeighted; |
| 310 | debugOut.analog[24] = accVector; |
308 | debugOut.analog[24] = accVector; |
| - | 309 | ||
| - | 310 | accPart /= controlActivityWeighted; |
|
| - | 311 | accPart /= accVectorWeighted; |
|
| - | 312 | ||
| - | 313 | if (controlActivityFlash < controlActivityWeighted) { |
|
| - | 314 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
|
| - | 315 | } else { |
|
| - | 316 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
|
| - | 317 | } |
|
| - | 318 | if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1; |
|
| - | 319 | ||
| - | 320 | if (accFlash < accVectorWeighted) { |
|
| - | 321 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
|
| - | 322 | } else { |
|
| - | 323 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
|
| 311 | 324 | } |
|
| 312 | accPart /= controlActivityWeighted; |
325 | if (++accFlash > CF_MAX+1) accFlash=1; |
| 313 | accPart /= accVectorWeighted; |
326 | |
| 314 | /* |
327 | /* |
| 315 | * Add to each sum: The amount by which the angle is changed just below. |
328 | * Add to each sum: The amount by which the angle is changed just below. |