| 308,16 → 308,25 |
|---|
| } |
| |
| void measureGyroActivity(int16_t newValue) { |
| gyroActivity += abs(newValue); |
| gyroActivity += (uint32_t)((int32_t)newValue * newValue); |
| } |
| |
| #define GADAMPING 10 |
| #define GADAMPING 6 |
| void dampenGyroActivity(void) { |
| uint32_t tmp = gyroActivity; |
| tmp *= ((1<<GADAMPING)-1); |
| tmp >>= GADAMPING; |
| gyroActivity = tmp; |
| static uint8_t cnt = 0; |
| if (++cnt >= IMUConfig.gyroActivityDamping) { |
| cnt = 0; |
| gyroActivity *= (uint32_t)((1L<<GADAMPING)-1); |
| gyroActivity >>= GADAMPING; |
| } |
| } |
| /* |
| void dampenGyroActivity(void) { |
| if (gyroActivity >= 2000) { |
| gyroActivity -= 2000; |
| } |
| } |
| */ |
| |
| void analog_updateGyros(void) { |
| // for various filters... |
| 370,10 → 379,6 |
|---|
| tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
| } |
| |
| if (gyroDWindowIdx >= GYRO_D_WINDOW_LENGTH) { |
| gyroDWindowIdx = 0; |
| } |
| |
| /* |
| * Now process the data for attitude angles. |
| */ |
| 383,16 → 388,34 |
|---|
| gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
| gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
| |
| /* |
| measureGyroActivity(tempOffsetGyro[PITCH]); |
| measureGyroActivity(tempOffsetGyro[ROLL]); |
| */ |
| measureGyroActivity(gyroD[PITCH]); |
| measureGyroActivity(gyroD[ROLL]); |
| |
| // Yaw gyro. |
| // We measure activity of yaw by plain gyro value and not d/dt, because: |
| // - There is no drift correction anyway |
| // - Effect of steady circular flight would vanish (it should have effect). |
| // int16_t diff = yawGyro; |
| // Yaw gyro. |
| if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
| yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
| else |
| yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
| |
| measureGyroActivity(yawGyro*IMUConfig.yawRateFactor); |
| // diff -= yawGyro; |
| // gyroD[YAW] -= gyroDWindow[YAW][gyroDWindowIdx]; |
| // gyroD[YAW] += diff; |
| // gyroDWindow[YAW][gyroDWindowIdx] = diff; |
| |
| // gyroActivity += (uint32_t)(abs(yawGyro)* IMUConfig.yawRateFactor); |
| measureGyroActivity(yawGyro); |
| |
| if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
| gyroDWindowIdx = 0; |
| } |
| } |
| |
| void analog_updateAccelerometers(void) { |