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 |