Rev 2092 | Rev 2097 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2092 | Rev 2095 | ||
|---|---|---|---|
| Line 306... | Line 306... | ||
| 306 | // do not restart ADC converter. |
306 | // do not restart ADC converter. |
| 307 | } |
307 | } |
| 308 | } |
308 | } |
| Line 309... | Line 309... | ||
| 309 | 309 | ||
| 310 | void measureGyroActivity(int16_t newValue) { |
310 | void measureGyroActivity(int16_t newValue) { |
| 311 | gyroActivity += abs(newValue); |
311 | gyroActivity += (uint32_t)((int32_t)newValue * newValue); |
| Line 312... | Line 312... | ||
| 312 | } |
312 | } |
| 313 | 313 | ||
| 314 | #define GADAMPING 10 |
314 | #define GADAMPING 6 |
| 315 | void dampenGyroActivity(void) { |
315 | void dampenGyroActivity(void) { |
| 316 | uint32_t tmp = gyroActivity; |
316 | static uint8_t cnt = 0; |
| - | 317 | if (++cnt >= IMUConfig.gyroActivityDamping) { |
|
| 317 | tmp *= ((1<<GADAMPING)-1); |
318 | cnt = 0; |
| - | 319 | gyroActivity *= (uint32_t)((1L<<GADAMPING)-1); |
|
| 318 | tmp >>= GADAMPING; |
320 | gyroActivity >>= GADAMPING; |
| - | 321 | } |
|
| - | 322 | } |
|
| - | 323 | /* |
|
| - | 324 | void dampenGyroActivity(void) { |
|
| - | 325 | if (gyroActivity >= 2000) { |
|
| - | 326 | gyroActivity -= 2000; |
|
| - | 327 | } |
|
| Line 319... | Line 328... | ||
| 319 | gyroActivity = tmp; |
328 | } |
| 320 | } |
329 | */ |
| 321 | 330 | ||
| Line 368... | Line 377... | ||
| 368 | 377 | ||
| 369 | // Prepare tempOffsetGyro for next calculation below... |
378 | // Prepare tempOffsetGyro for next calculation below... |
| 370 | tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
379 | tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
| Line 371... | Line -... | ||
| 371 | } |
- | |
| 372 | - | ||
| 373 | if (gyroDWindowIdx >= GYRO_D_WINDOW_LENGTH) { |
- | |
| 374 | gyroDWindowIdx = 0; |
- | |
| 375 | } |
380 | } |
| 376 | 381 | ||
| 377 | /* |
382 | /* |
| 378 | * Now process the data for attitude angles. |
383 | * Now process the data for attitude angles. |
| Line 379... | Line 384... | ||
| 379 | */ |
384 | */ |
| 380 | rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
385 | rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
| 381 | 386 | ||
| Line -... | Line 387... | ||
| - | 387 | dampenGyroActivity(); |
|
| 382 | dampenGyroActivity(); |
388 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
| 383 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
389 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
| - | 390 | ||
| - | 391 | /* |
|
| - | 392 | measureGyroActivity(tempOffsetGyro[PITCH]); |
|
| Line -... | Line 393... | ||
| - | 393 | measureGyroActivity(tempOffsetGyro[ROLL]); |
|
| - | 394 | */ |
|
| - | 395 | measureGyroActivity(gyroD[PITCH]); |
|
| - | 396 | measureGyroActivity(gyroD[ROLL]); |
|
| 384 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
397 | |
| 385 | 398 | // We measure activity of yaw by plain gyro value and not d/dt, because: |
|
| 386 | measureGyroActivity(tempOffsetGyro[PITCH]); |
399 | // - There is no drift correction anyway |
| 387 | measureGyroActivity(tempOffsetGyro[ROLL]); |
400 | // - Effect of steady circular flight would vanish (it should have effect). |
| 388 | 401 | // int16_t diff = yawGyro; |
|
| Line -... | Line 402... | ||
| - | 402 | // Yaw gyro. |
|
| - | 403 | if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
|
| - | 404 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
|
| - | 405 | else |
|
| - | 406 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
|
| 389 | // Yaw gyro. |
407 | |
| - | 408 | // diff -= yawGyro; |
|
| - | 409 | // gyroD[YAW] -= gyroDWindow[YAW][gyroDWindowIdx]; |
|
| - | 410 | // gyroD[YAW] += diff; |
|
| - | 411 | // gyroDWindow[YAW][gyroDWindowIdx] = diff; |
|
| - | 412 | ||
| 390 | if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
413 | // gyroActivity += (uint32_t)(abs(yawGyro)* IMUConfig.yawRateFactor); |
| Line 391... | Line 414... | ||
| 391 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
414 | measureGyroActivity(yawGyro); |
| 392 | else |
415 | |
| 393 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
416 | if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |