Rev 2052 | Rev 2069 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2052 | Rev 2055 | ||
|---|---|---|---|
| Line 295... | Line 295... | ||
| 295 | analogDataReady = 1; |
295 | analogDataReady = 1; |
| 296 | // do not restart ADC converter. |
296 | // do not restart ADC converter. |
| 297 | } |
297 | } |
| 298 | } |
298 | } |
| Line -... | Line 299... | ||
| - | 299 | ||
| - | 300 | // Experimental gyro shake takeoff detect! |
|
| - | 301 | uint16_t gyroActivity = 0; |
|
| - | 302 | void measureGyroActivityAndUpdateGyro(uint8_t axis, int16_t newValue) { |
|
| - | 303 | int16_t tmp = gyro_ATT[axis]; |
|
| - | 304 | gyro_ATT[axis] = newValue; |
|
| - | 305 | ||
| - | 306 | tmp -= newValue; |
|
| - | 307 | tmp = (tmp*tmp) >> 4; |
|
| - | 308 | ||
| - | 309 | if (gyroActivity + (uint16_t)tmp < 0x8000) |
|
| - | 310 | gyroActivity += tmp; |
|
| - | 311 | } |
|
| - | 312 | ||
| - | 313 | #define GADAMPING 10 |
|
| - | 314 | void dampenGyroActivity(void) { |
|
| - | 315 | uint32_t tmp = gyroActivity; |
|
| - | 316 | tmp *= ((1<<GADAMPING)-1); |
|
| - | 317 | tmp >>= GADAMPING; |
|
| - | 318 | gyroActivity = tmp; |
|
| - | 319 | } |
|
| 299 | 320 | ||
| 300 | void analog_updateGyros(void) { |
321 | void analog_updateGyros(void) { |
| 301 | // for various filters... |
322 | // for various filters... |
| Line 302... | Line 323... | ||
| 302 | int16_t tempOffsetGyro[2], tempGyro; |
323 | int16_t tempOffsetGyro[2], tempGyro; |
| Line 347... | Line 368... | ||
| 347 | /* |
368 | /* |
| 348 | * Now process the data for attitude angles. |
369 | * Now process the data for attitude angles. |
| 349 | */ |
370 | */ |
| 350 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
371 | rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
| Line 351... | Line 372... | ||
| 351 | 372 | ||
| 352 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
373 | measureGyroActivityAndUpdateGyro(0, tempOffsetGyro[PITCH]); |
| 353 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
- | |
| 354 | - | ||
| 355 | debugOut.analog[22 + 0] = gyro_PID[0]; |
- | |
| 356 | debugOut.analog[22 + 1] = gyro_PID[1]; |
- | |
| 357 | - | ||
| 358 | debugOut.analog[24 + 0] = gyro_ATT[0]; |
374 | measureGyroActivityAndUpdateGyro(1, tempOffsetGyro[ROLL]); |
| 359 | debugOut.analog[24 + 1] = gyro_ATT[1]; |
- | |
| 360 | - | ||
| 361 | // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else. |
- | |
| 362 | // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter; |
- | |
| Line 363... | Line 375... | ||
| 363 | // gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter; |
375 | dampenGyroActivity(); |
| 364 | 376 | ||
| 365 | // Yaw gyro. |
377 | // Yaw gyro. |
| 366 | if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
378 | if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
| Line 425... | Line 437... | ||
| 425 | } |
437 | } |
| 426 | } |
438 | } |
| Line 427... | Line 439... | ||
| 427 | 439 | ||
| 428 | // Even if the sample is off-range, use it. |
440 | // Even if the sample is off-range, use it. |
| - | 441 | simpleAirPressure = getSimplePressure(rawAirPressure); |
|
| - | 442 | debugOut.analog[6] = rawAirPressure; |
|
| Line 429... | Line 443... | ||
| 429 | simpleAirPressure = getSimplePressure(rawAirPressure); |
443 | debugOut.analog[7] = simpleAirPressure; |
| 430 | 444 | ||
| 431 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
445 | if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
| 432 | // Danger: pressure near lower end of range. If the measurement saturates, the |
446 | // Danger: pressure near lower end of range. If the measurement saturates, the |
| Line 488... | Line 502... | ||
| 488 | analog_updateGyros(); |
502 | analog_updateGyros(); |
| 489 | analog_updateAccelerometers(); |
503 | analog_updateAccelerometers(); |
| 490 | analog_updateAirPressure(); |
504 | analog_updateAirPressure(); |
| 491 | analog_updateBatteryVoltage(); |
505 | analog_updateBatteryVoltage(); |
| 492 | #ifdef USE_MK3MAG |
506 | #ifdef USE_MK3MAG |
| 493 | debugOut.analog[12] = magneticHeading = volatileMagneticHeading; |
507 | magneticHeading = volatileMagneticHeading; |
| 494 | #endif |
508 | #endif |
| 495 | } |
509 | } |
| Line 496... | Line 510... | ||
| 496 | 510 | ||
| 497 | void analog_setNeutral() { |
511 | void analog_setNeutral() { |
| Line 515... | Line 529... | ||
| 515 | 529 | ||
| 516 | // Setting offset values has an influence in the analog.c ISR |
530 | // Setting offset values has an influence in the analog.c ISR |
| 517 | // Therefore run measurement for 100ms to achive stable readings |
531 | // Therefore run measurement for 100ms to achive stable readings |
| Line 518... | Line -... | ||
| 518 | delay_ms_with_adc_measurement(100, 0); |
- | |
| 519 | - | ||
| 520 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
532 | delay_ms_with_adc_measurement(100, 0); |
| 521 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2); |
533 | |
| Line 522... | Line 534... | ||
| 522 | // pressureMeasurementCount = 0; |
534 | gyroActivity = 0; |
| 523 | } |
535 | } |
| 524 | 536 | ||