Rev 1965 | Rev 1969 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1965 | Rev 1967 | ||
|---|---|---|---|
| Line 103... | Line 103... | ||
| 103 | * to be centered on zero. |
103 | * to be centered on zero. |
| 104 | */ |
104 | */ |
| Line 105... | Line 105... | ||
| 105 | 105 | ||
| 106 | volatile sensorOffset_t gyroOffset; |
106 | volatile sensorOffset_t gyroOffset; |
| 107 | volatile sensorOffset_t accOffset; |
107 | volatile sensorOffset_t accOffset; |
| Line 108... | Line 108... | ||
| 108 | volatile sensorOffset_t DACValues; |
108 | volatile sensorOffset_t gyroAmplifierOffset; |
| 109 | 109 | ||
| 110 | /* |
110 | /* |
| 111 | * This allows some experimentation with the gyro filters. |
111 | * This allows some experimentation with the gyro filters. |
| Line 279... | Line 279... | ||
| 279 | // for various filters... |
279 | // for various filters... |
| 280 | int16_t tempOffsetGyro, tempGyro; |
280 | int16_t tempOffsetGyro, tempGyro; |
| Line 281... | Line 281... | ||
| 281 | 281 | ||
| 282 | for (uint8_t axis=0; axis<2; axis++) { |
282 | for (uint8_t axis=0; axis<2; axis++) { |
| - | 283 | tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
|
| 283 | tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis]; |
284 | |
| 284 | /* |
285 | /* |
| 285 | * Process the gyro data for the PID controller. |
286 | * Process the gyro data for the PID controller. |
| 286 | */ |
287 | */ |
| 287 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
288 | // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
| Line 340... | Line 341... | ||
| 340 | if (GYRO_REVERSED[YAW]) |
341 | if (GYRO_REVERSED[YAW]) |
| 341 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
342 | yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
| 342 | else |
343 | else |
| 343 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
344 | yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
| Line 344... | Line 345... | ||
| 344 | 345 | ||
| 345 | debugOut.analog[3] = gyro_ATT[PITCH]; |
346 | debugOut.analog[3] = rawGyroSum[0]; |
| 346 | debugOut.analog[4] = gyro_ATT[ROLL]; |
347 | debugOut.analog[4] = rawGyroSum[1]; |
| 347 | debugOut.analog[5] = yawGyro; |
348 | debugOut.analog[5] = rawGyroSum[2]; |
| Line 348... | Line 349... | ||
| 348 | } |
349 | } |
| 349 | 350 | ||
| 350 | void analog_updateAccelerometers(void) { |
351 | void analog_updateAccelerometers(void) { |
| Line 469... | Line 470... | ||
| 469 | analog_updateAirPressure(); |
470 | analog_updateAirPressure(); |
| 470 | analog_updateBatteryVoltage(); |
471 | analog_updateBatteryVoltage(); |
| 471 | } |
472 | } |
| Line 472... | Line 473... | ||
| 472 | 473 | ||
| - | 474 | void analog_setNeutral() { |
|
| - | 475 | if (gyroAmplifierOffset_readFromEEProm()) { |
|
| - | 476 | printf("gyro amp invalid, you must recalibrate."); |
|
| - | 477 | gyro_loadOffsets(1); |
|
| - | 478 | } |
|
| 473 | void analog_setNeutral() { |
479 | |
| 474 | if (gyroOffset_readFromEEProm()) { |
480 | if (gyroOffset_readFromEEProm()) { |
| 475 | printf("gyro offsets invalid, you must recalibrate."); |
481 | printf("gyro offsets invalid, you must recalibrate."); |
| 476 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
482 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
| 477 | gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
483 | gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
| Line 490... | Line 496... | ||
| 490 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
496 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
| 491 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
497 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
| Line 492... | Line 498... | ||
| 492 | 498 | ||
| 493 | // Setting offset values has an influence in the analog.c ISR |
499 | // Setting offset values has an influence in the analog.c ISR |
| 494 | // Therefore run measurement for 100ms to achive stable readings |
500 | // Therefore run measurement for 100ms to achive stable readings |
| Line 495... | Line 501... | ||
| 495 | delay_ms_Mess(100); |
501 | delay_ms_with_adc_measurement(100); |
| 496 | 502 | ||
| 497 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
503 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
| 498 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
504 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
| Line 505... | Line 511... | ||
| 505 | int32_t offsets[3] = { 0, 0, 0 }; |
511 | int32_t offsets[3] = { 0, 0, 0 }; |
| 506 | gyro_calibrate(); |
512 | gyro_calibrate(); |
| Line 507... | Line 513... | ||
| 507 | 513 | ||
| 508 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
514 | // determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
| 509 | for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
515 | for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
| 510 | delay_ms_Mess(20); |
516 | delay_ms_with_adc_measurement(20); |
| 511 | for (axis = PITCH; axis <= YAW; axis++) { |
517 | for (axis = PITCH; axis <= YAW; axis++) { |
| 512 | offsets[axis] += rawGyroSum[axis]; |
518 | offsets[axis] += rawGyroSum[axis]; |
| 513 | } |
519 | } |
| Line 532... | Line 538... | ||
| 532 | uint8_t i, axis; |
538 | uint8_t i, axis; |
| 533 | int32_t deltaOffset[3] = { 0, 0, 0 }; |
539 | int32_t deltaOffset[3] = { 0, 0, 0 }; |
| 534 | int16_t filteredDelta; |
540 | int16_t filteredDelta; |
| Line 535... | Line 541... | ||
| 535 | 541 | ||
| 536 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
542 | for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
| 537 | delay_ms_Mess(10); |
543 | delay_ms_with_adc_measurement(10); |
| 538 | for (axis = PITCH; axis <= YAW; axis++) { |
544 | for (axis = PITCH; axis <= YAW; axis++) { |
| 539 | deltaOffset[axis] += acc[axis]; |
545 | deltaOffset[axis] += acc[axis]; |
| 540 | } |
546 | } |