Rev 2099 | Rev 2103 | Go to most recent revision | Show entire file | Regard 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) { |