Rev 1960 | Rev 1963 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1960 | Rev 1961 | ||
|---|---|---|---|
| Line 98... | Line 98... | ||
| 98 | /* |
98 | /* |
| 99 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
99 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| 100 | * standing still. They are used for adjusting the gyro and acc. meter values |
100 | * standing still. They are used for adjusting the gyro and acc. meter values |
| 101 | * to be centered on zero. |
101 | * to be centered on zero. |
| 102 | */ |
102 | */ |
| 103 | /* |
- | |
| 104 | volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 |
- | |
| 105 | * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW }; |
- | |
| 106 | - | ||
| 107 | volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
- | |
| 108 | * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
- | |
| 109 | */ |
- | |
| Line 110... | Line 103... | ||
| 110 | 103 | ||
| 111 | sensorOffset_t gyroOffset; |
104 | sensorOffset_t gyroOffset; |
| Line 112... | Line 105... | ||
| 112 | sensorOffset_t accOffset; |
105 | sensorOffset_t accOffset; |
| Line 472... | Line 465... | ||
| 472 | analog_updateAccelerometers(); |
465 | analog_updateAccelerometers(); |
| 473 | analog_updateAirPressure(); |
466 | analog_updateAirPressure(); |
| 474 | analog_updateBatteryVoltage(); |
467 | analog_updateBatteryVoltage(); |
| 475 | } |
468 | } |
| Line -... | Line 469... | ||
| - | 469 | ||
| - | 470 | void analog_setNeutral() { |
|
| - | 471 | if (gyroOffset_readFromEEProm()) { |
|
| - | 472 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
|
| - | 473 | gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
|
| - | 474 | } |
|
| - | 475 | ||
| - | 476 | if (accOffset_readFromEEProm()) { |
|
| - | 477 | accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL; |
|
| - | 478 | accOffset.offsets[Z] = 512 * ACC_SUMMATION_FACTOR_Z; |
|
| - | 479 | } |
|
| - | 480 | ||
| - | 481 | // Noise is relative to offset. So, reset noise measurements when changing offsets. |
|
| - | 482 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
|
| - | 483 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
|
| - | 484 | ||
| - | 485 | // Setting offset values has an influence in the analog.c ISR |
|
| - | 486 | // Therefore run measurement for 100ms to achive stable readings |
|
| - | 487 | delay_ms_Mess(100); |
|
| - | 488 | ||
| - | 489 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
|
| - | 490 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
|
| - | 491 | // pressureMeasurementCount = 0; |
|
| - | 492 | } |
|
| 476 | 493 | ||
| 477 | void analog_calibrate(void) { |
494 | void analog_calibrateGyros(void) { |
| 478 | #define GYRO_OFFSET_CYCLES 32 |
495 | #define GYRO_OFFSET_CYCLES 32 |
| 479 | uint8_t i, axis; |
496 | uint8_t i, axis; |
| 480 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
497 | int32_t deltaOffsets[3] = { 0, 0, 0 }; |
| Line 490... | Line 507... | ||
| 490 | 507 | ||
| 491 | for (axis = PITCH; axis <= YAW; axis++) { |
508 | for (axis = PITCH; axis <= YAW; axis++) { |
| 492 | gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
509 | gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
| 493 | debugOut.analog[6+axis] = gyroOffset.offsets[axis]; |
510 | debugOut.analog[6+axis] = gyroOffset.offsets[axis]; |
| 494 | } |
511 | } |
| 495 | - | ||
| 496 | // Noise is relativ to offset. So, reset noise measurements when changing offsets. |
- | |
| 497 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
- | |
| 498 | 512 | ||
| 499 | accOffset_readFromEEProm(); |
- | |
| 500 | // accOffset[PITCH] = getParamWord(PID_ACC_PITCH); |
- | |
| 501 | // accOffset[ROLL] = getParamWord(PID_ACC_ROLL); |
- | |
| 502 | // accOffset[Z] = getParamWord(PID_ACC_Z); |
- | |
| 503 | - | ||
| 504 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
- | |
| 505 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
- | |
| 506 | // pressureMeasurementCount = 0; |
- | |
| 507 | - | ||
| 508 | delay_ms_Mess(100); |
513 | gyroOffset_writeToEEProm(); |
| Line 509... | Line 514... | ||
| 509 | } |
514 | } |
| 510 | 515 | ||
| 511 | /* |
516 | /* |
| Line 532... | Line 537... | ||
| 532 | for (axis = PITCH; axis <= YAW; axis++) { |
537 | for (axis = PITCH; axis <= YAW; axis++) { |
| 533 | filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
538 | filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
| 534 | / ACC_OFFSET_CYCLES; |
539 | / ACC_OFFSET_CYCLES; |
| 535 | accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
540 | accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
| 536 | } |
541 | } |
| 537 | - | ||
| 538 | // Save ACC neutral settings to eeprom |
- | |
| 539 | // setParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
- | |
| 540 | // setParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
- | |
| 541 | // setParamWord(PID_ACC_Z, accOffset[Z]); |
- | |
| 542 | accOffset_writeToEEProm(); |
- | |
| Line 543... | Line -... | ||
| 543 | - | ||
| 544 | // Noise is relative to offset. So, reset noise measurements when |
- | |
| 545 | // changing offsets. |
- | |
| 546 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
- | |
| 547 | - | ||
| 548 | // Setting offset values has an influence in the analog.c ISR |
- | |
| 549 | // Therefore run measurement for 100ms to achive stable readings |
542 | |
| 550 | delay_ms_Mess(100); |
543 | accOffset_writeToEEProm(); |