Rev 2088 | Rev 2095 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2088 | Rev 2089 | ||
---|---|---|---|
Line 48... | Line 48... | ||
48 | int16_t magneticHeading; |
48 | int16_t magneticHeading; |
Line 49... | Line 49... | ||
49 | 49 | ||
50 | int32_t groundPressure; |
50 | int32_t groundPressure; |
Line -... | Line 51... | ||
- | 51 | int16_t dHeight; |
|
- | 52 | ||
51 | int16_t dHeight; |
53 | uint32_t gyroActivity; |
52 | 54 | ||
53 | /* |
55 | /* |
54 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
56 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
55 | * standing still. They are used for adjusting the gyro and acc. meter values |
57 | * standing still. They are used for adjusting the gyro and acc. meter values |
Line 302... | Line 304... | ||
302 | analogDataReady = 1; |
304 | analogDataReady = 1; |
303 | // do not restart ADC converter. |
305 | // do not restart ADC converter. |
304 | } |
306 | } |
305 | } |
307 | } |
Line 306... | Line -... | ||
306 | - | ||
307 | // Experimental gyro shake takeoff detect! |
- | |
308 | uint16_t gyroActivity = 0; |
308 | |
309 | void measureGyroActivityAndUpdateGyro(uint8_t axis, int16_t newValue) { |
- | |
310 | int16_t tmp = gyro_ATT[axis]; |
309 | void measureGyroActivity(int16_t newValue) { |
311 | gyro_ATT[axis] = newValue; |
- | |
312 | - | ||
313 | tmp -= newValue; |
- | |
314 | tmp = (tmp*tmp) >> 4; |
- | |
315 | - | ||
316 | if (gyroActivity + (uint16_t)tmp < 0x8000) |
- | |
317 | gyroActivity += tmp; |
310 | gyroActivity += abs(newValue); |
Line 318... | Line 311... | ||
318 | } |
311 | } |
319 | 312 | ||
320 | #define GADAMPING 10 |
313 | #define GADAMPING 10 |
Line 383... | Line 376... | ||
383 | /* |
376 | /* |
384 | * Now process the data for attitude angles. |
377 | * Now process the data for attitude angles. |
385 | */ |
378 | */ |
386 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
379 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
Line 387... | Line -... | ||
387 | - | ||
388 | measureGyroActivityAndUpdateGyro(0, tempOffsetGyro[PITCH]); |
- | |
389 | measureGyroActivityAndUpdateGyro(1, tempOffsetGyro[ROLL]); |
380 | |
- | 381 | dampenGyroActivity(); |
|
- | 382 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
|
- | 383 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
|
- | 384 | ||
- | 385 | measureGyroActivity(tempOffsetGyro[PITCH]); |
|
Line 390... | Line 386... | ||
390 | dampenGyroActivity(); |
386 | measureGyroActivity(tempOffsetGyro[ROLL]); |
391 | 387 | ||
392 | // Yaw gyro. |
388 | // Yaw gyro. |
393 | if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
389 | if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
394 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
390 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
- | 391 | else |
|
- | 392 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
|
395 | else |
393 | |
Line 396... | Line 394... | ||
396 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
394 | measureGyroActivity(yawGyro*staticParams.yawRateFactor); |
397 | } |
395 | } |
398 | 396 | ||
Line 412... | Line 410... | ||
412 | if (staticParams.imuReversedFlags & 8) |
410 | if (staticParams.imuReversedFlags & 8) |
413 | acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
411 | acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
414 | else |
412 | else |
415 | acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
413 | acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
Line 416... | Line 414... | ||
416 | 414 | ||
417 | debugOut.analog[29] = acc[Z]; |
415 | // debugOut.analog[29] = acc[Z]; |
Line 418... | Line 416... | ||
418 | } |
416 | } |
419 | 417 | ||
420 | void analog_updateAirPressure(void) { |
418 | void analog_updateAirPressure(void) { |