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) { |