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