| 50,6 → 50,8 |
|---|
| int32_t groundPressure; |
| int16_t dHeight; |
| |
| uint32_t gyroActivity; |
| |
| /* |
| * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| * standing still. They are used for adjusting the gyro and acc. meter values |
| 304,17 → 306,8 |
|---|
| } |
| } |
| |
| // Experimental gyro shake takeoff detect! |
| uint16_t gyroActivity = 0; |
| void measureGyroActivityAndUpdateGyro(uint8_t axis, int16_t newValue) { |
| int16_t tmp = gyro_ATT[axis]; |
| gyro_ATT[axis] = newValue; |
| |
| tmp -= newValue; |
| tmp = (tmp*tmp) >> 4; |
| |
| if (gyroActivity + (uint16_t)tmp < 0x8000) |
| gyroActivity += tmp; |
| void measureGyroActivity(int16_t newValue) { |
| gyroActivity += abs(newValue); |
| } |
| |
| #define GADAMPING 10 |
| 385,15 → 378,20 |
|---|
| */ |
| rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
| |
| measureGyroActivityAndUpdateGyro(0, tempOffsetGyro[PITCH]); |
| measureGyroActivityAndUpdateGyro(1, tempOffsetGyro[ROLL]); |
| dampenGyroActivity(); |
| gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
| gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
| |
| measureGyroActivity(tempOffsetGyro[PITCH]); |
| measureGyroActivity(tempOffsetGyro[ROLL]); |
| |
| // Yaw gyro. |
| if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
| yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
| else |
| yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
| |
| measureGyroActivity(yawGyro*staticParams.yawRateFactor); |
| } |
| |
| void analog_updateAccelerometers(void) { |
| 414,7 → 412,7 |
|---|
| else |
| acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
| |
| debugOut.analog[29] = acc[Z]; |
| // debugOut.analog[29] = acc[Z]; |
| } |
| |
| void analog_updateAirPressure(void) { |