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 | } |