Rev 2099 | Rev 2103 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2099 | Rev 2102 | ||
|---|---|---|---|
| Line 5... | Line 5... | ||
| 5 | 5 | ||
| 6 | #include "analog.h" |
6 | #include "analog.h" |
| 7 | #include "attitude.h" |
7 | #include "attitude.h" |
| 8 | #include "sensors.h" |
8 | #include "sensors.h" |
| - | 9 | #include "printf_P.h" |
|
| Line 9... | Line 10... | ||
| 9 | #include "printf_P.h" |
10 | #include "isqrt.h" |
| 10 | 11 | ||
| Line 11... | Line 12... | ||
| 11 | // for Delay functions |
12 | // for Delay functions |
| Line 42... | Line 43... | ||
| 42 | int16_t gyro_PID[3]; |
43 | int16_t gyro_PID[3]; |
| 43 | int16_t gyro_ATT[3]; |
44 | int16_t gyro_ATT[3]; |
| 44 | int16_t gyroD[3]; |
45 | int16_t gyroD[3]; |
| 45 | int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH]; |
46 | int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH]; |
| 46 | uint8_t gyroDWindowIdx = 0; |
47 | uint8_t gyroDWindowIdx = 0; |
| 47 | int16_t dHeight; |
48 | //int16_t dHeight; |
| 48 | uint32_t gyroActivity; |
49 | //uint32_t gyroActivity; |
| Line 49... | Line 50... | ||
| 49 | 50 | ||
| 50 | /* |
51 | /* |
| 51 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
52 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| 52 | * standing still. They are used for adjusting the gyro and acc. meter values |
53 | * standing still. They are used for adjusting the gyro and acc. meter values |
| Line 114... | Line 115... | ||
| 114 | 115 | ||
| 115 | /* |
116 | /* |
| 116 | * Airspeed |
117 | * Airspeed |
| 117 | */ |
118 | */ |
| - | 119 | uint16_t simpleAirPressure; |
|
| Line 118... | Line 120... | ||
| 118 | uint16_t simpleAirPressure; |
120 | uint16_t airspeedVelocity; |
| 119 | 121 | ||
| Line 120... | Line 122... | ||
| 120 | // Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
122 | // Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
| Line 373... | Line 375... | ||
| 373 | 375 | ||
| 374 | // dampenGyroActivity(); |
376 | // dampenGyroActivity(); |
| 375 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
377 | gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
| Line 376... | Line -... | ||
| 376 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
- | |
| 377 | - | ||
| 378 | /* |
- | |
| 379 | measureGyroActivity(gyroD[PITCH]); |
- | |
| 380 | measureGyroActivity(gyroD[ROLL]); |
- | |
| 381 | measureGyroActivity(yawGyro); |
- | |
| 382 | */ |
378 | gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
| 383 | 379 | ||
| 384 | if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
380 | if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
| 385 | gyroDWindowIdx = 0; |
381 | gyroDWindowIdx = 0; |
| Line 386... | Line 382... | ||
| 386 | } |
382 | } |
| 387 | } |
383 | } |
| 388 | 384 | ||
| - | 385 | void analog_updateAirPressure(void) { |
|
| 389 | void analog_updateAirPressure(void) { |
386 | uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
| Line 390... | Line 387... | ||
| 390 | uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
387 | simpleAirPressure = rawAirPressure; |
| 391 | simpleAirPressure = rawAirPressure; |
388 | airspeedVelocity = isqrt16(simpleAirPressure); |
| 392 | } |
389 | } |
| Line 415... | Line 412... | ||
| 415 | printf("gyro offsets invalid%s",recal); |
412 | printf("gyro offsets invalid%s",recal); |
| 416 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING; |
413 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING; |
| 417 | gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING; |
414 | gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING; |
| 418 | } |
415 | } |
| Line 419... | Line -... | ||
| 419 | - | ||
| 420 | /* |
- | |
| 421 | if (accOffset_readFromEEProm()) { |
- | |
| 422 | printf("acc. meter offsets invalid%s",recal); |
- | |
| 423 | accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
- | |
| 424 | accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
- | |
| 425 | } |
- | |
| 426 | */ |
- | |
| 427 | 416 | ||
| 428 | // Noise is relative to offset. So, reset noise measurements when changing offsets. |
417 | // Noise is relative to offset. So, reset noise measurements when changing offsets. |
| 429 | for (uint8_t i=PITCH; i<=YAW; i++) { |
418 | for (uint8_t i=PITCH; i<=YAW; i++) { |
| 430 | gyroNoisePeak[i] = 0; |
419 | gyroNoisePeak[i] = 0; |
| 431 | gyroD[i] = 0; |
420 | gyroD[i] = 0; |
| Line 435... | Line 424... | ||
| 435 | } |
424 | } |
| 436 | // Setting offset values has an influence in the analog.c ISR |
425 | // Setting offset values has an influence in the analog.c ISR |
| 437 | // Therefore run measurement for 100ms to achive stable readings |
426 | // Therefore run measurement for 100ms to achive stable readings |
| 438 | delay_ms_with_adc_measurement(100, 0); |
427 | delay_ms_with_adc_measurement(100, 0); |
| Line 439... | Line 428... | ||
| 439 | 428 | ||
| 440 | gyroActivity = 0; |
429 | // gyroActivity = 0; |
| Line 441... | Line 430... | ||
| 441 | } |
430 | } |
| 442 | 431 | ||
| 443 | void analog_calibrateGyros(void) { |
432 | void analog_calibrateGyros(void) { |