Rev 2088 | Rev 2092 | 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) { |