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(); |