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