Rev 2112 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1967 | - | 1 | #include "sensors.h" |
| 1612 | dongfang | 2 | #include "analog.h" |
| 3 | #include "twimaster.h" |
||
| 4 | #include "configuration.h" |
||
| 1969 | - | 5 | #include "eeprom.h" |
| 1612 | dongfang | 6 | #include "timer0.h" |
| 2189 | - | 7 | #include "output.h" |
| 1612 | dongfang | 8 | |
| 2189 | - | 9 | #include <stdio.h> |
| 10 | |||
| 2092 | - | 11 | void I2C_OutputAmplifierOffsets(void) { |
| 2018 | - | 12 | uint16_t timeout = setDelay(2000); |
| 13 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
||
| 14 | // Wait for I2C to finish transmission. |
||
| 15 | while (twi_state) { |
||
| 16 | // Did it take too long? |
||
| 17 | if (checkDelay(timeout)) { |
||
| 18 | printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
||
| 19 | break; |
||
| 20 | } |
||
| 21 | } |
||
| 22 | } |
||
| 23 | |||
| 2189 | - | 24 | uint8_t DACChannelFor(uint8_t axis) { |
| 25 | switch (axis) { |
||
| 26 | case X: |
||
| 27 | return 1; |
||
| 28 | case Y: |
||
| 29 | return 0; |
||
| 30 | case Z: |
||
| 31 | return 2; |
||
| 32 | default: |
||
| 33 | return -1; // should never happen. |
||
| 34 | } |
||
| 35 | } |
||
| 36 | |||
| 1967 | - | 37 | void gyro_calibrate(void) { |
| 38 | printf("gyro_calibrate"); |
||
| 1821 | - | 39 | uint8_t i, axis, factor, numberOfAxesInRange = 0; |
| 40 | // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
||
| 2189 | - | 41 | |
| 42 | for (i = 200; i != 0; i--) { |
||
| 43 | waitADCCycle(i <= 10 ? 10 : 2); |
||
| 1821 | - | 44 | // If all 3 axis are in range, shorten the remaining number of iterations. |
| 2015 | - | 45 | if (numberOfAxesInRange == 3 && i > 10) i = 10; |
| 1821 | - | 46 | numberOfAxesInRange = 0; |
| 2189 | - | 47 | for (axis=X; axis<=Z; axis++) { |
| 48 | uint8_t dacChannel = DACChannelFor(axis); |
||
| 49 | if (axis==Z) |
||
| 50 | factor=GYRO_OVERSAMPLING_Z; |
||
| 1821 | - | 51 | else |
| 2189 | - | 52 | factor=GYRO_OVERSAMPLING_XY; |
| 1821 | - | 53 | |
| 2189 | - | 54 | if (gyroValueForFC13DACCalibration(axis) < (uint16_t)(510*factor)) |
| 55 | gyroAmplifierOffset.offsets[dacChannel]--; |
||
| 56 | else if (gyroValueForFC13DACCalibration(axis) > (uint16_t)(515 * factor)) |
||
| 57 | gyroAmplifierOffset.offsets[dacChannel]++; |
||
| 1821 | - | 58 | else |
| 59 | numberOfAxesInRange++; |
||
| 60 | |||
| 61 | /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
||
| 2189 | - | 62 | if (gyroAmplifierOffset.offsets[dacChannel] < 10) { |
| 63 | gyroAmplifierOffset.offsets[dacChannel] = 10; |
||
| 64 | versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis); |
||
| 65 | } else if (gyroAmplifierOffset.offsets[dacChannel] > 245) { |
||
| 66 | gyroAmplifierOffset.offsets[dacChannel] = 245; |
||
| 67 | versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis); |
||
| 1821 | - | 68 | } |
| 69 | } |
||
| 1967 | - | 70 | |
| 2018 | - | 71 | I2C_OutputAmplifierOffsets(); |
| 1967 | - | 72 | } |
| 73 | gyroAmplifierOffset_writeToEEProm(); |
||
| 2189 | - | 74 | waitADCCycle(70); |
| 1967 | - | 75 | } |
| 1821 | - | 76 | |
| 2092 | - | 77 | void gyro_init(void) { |
| 2018 | - | 78 | if (gyroAmplifierOffset_readFromEEProm()) { |
| 2019 | - | 79 | printf("gyro amp invalid, recalibrate."); |
| 2189 | - | 80 | gyroAmplifierOffset.offsets[X] = |
| 81 | gyroAmplifierOffset.offsets[Y] = |
||
| 82 | gyroAmplifierOffset.offsets[Z] = (uint8_t)(255 * 1.2089 / 3.0); |
||
| 2018 | - | 83 | } else { |
| 84 | I2C_OutputAmplifierOffsets(); |
||
| 85 | } |
||
| 1612 | dongfang | 86 | } |
| 87 | |||
| 1971 | - | 88 | void gyro_setDefaultParameters(void) { |
| 2189 | - | 89 | IMUConfig.gyroQuadrant = 4; |
| 2092 | - | 90 | IMUConfig.accQuadrant = 4; |
| 2189 | - | 91 | IMUConfig.imuReversedFlags = IMU_REVERSE_ACCEL_Z | IMU_REVERSE_GYRO_Z; |
| 1960 | - | 92 | staticParams.gyroD = 3; |
| 1612 | dongfang | 93 | } |