Rev 1612 | Rev 1646 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1612 | dongfang | 1 | //#include "ENC-03_FC1.3.h" |
| 2 | #include "printf_P.h" |
||
| 3 | #include "analog.h" |
||
| 4 | #include "twimaster.h" |
||
| 5 | #include "configuration.h" |
||
| 6 | #include "timer0.h" |
||
| 7 | |||
| 1645 | - | 8 | const uint8_t GYROS_REVERSE[2] = {0,0}; |
| 1612 | dongfang | 9 | #define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510 |
| 10 | #define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515 |
||
| 11 | |||
| 12 | #define DAC_PITCH 0 |
||
| 13 | #define DAC_ROLL 1 |
||
| 14 | #define DAC_YAW 2 |
||
| 15 | |||
| 16 | // void gyro_init(void) {} |
||
| 17 | void gyro_calibrate(void) { |
||
| 18 | uint8_t i, numberOfAxesInRange = 0; |
||
| 19 | uint16_t timeout; |
||
| 20 | // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
||
| 21 | timeout = SetDelay(2000); |
||
| 22 | |||
| 23 | for(i = 140; i != 0; i--) { |
||
| 24 | // If all 3 axis are in range, shorten the remaining number of iterations. |
||
| 25 | if(numberOfAxesInRange == 3 && i > 10) i = 9; |
||
| 26 | |||
| 27 | numberOfAxesInRange = 0; |
||
| 28 | |||
| 1645 | - | 29 | if(rawGyroSum[PITCH] < PITCHROLL_MINLIMIT) DACValues[PITCH]--; |
| 30 | else if(rawGyroSum[PITCH] > PITCHROLL_MAXLIMIT) DACValues[PITCH]++; |
||
| 1612 | dongfang | 31 | else numberOfAxesInRange++; |
| 32 | |||
| 1645 | - | 33 | if(rawGyroSum[ROLL] < PITCHROLL_MINLIMIT) DACValues[ROLL]--; |
| 34 | else if(rawGyroSum[ROLL] > PITCHROLL_MAXLIMIT) DACValues[ROLL]++; |
||
| 1612 | dongfang | 35 | else numberOfAxesInRange++; |
| 36 | |||
| 37 | if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--; |
||
| 38 | else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ; |
||
| 39 | else numberOfAxesInRange++; |
||
| 40 | |||
| 1645 | - | 41 | if(DACValues[PITCH] < 10) { |
| 42 | /* GyroDefectNick = 1; */ DACValues[PITCH] = 10; |
||
| 43 | } else if(DACValues[PITCH] > 245) { |
||
| 44 | /* GyroDefectNick = 1; */ DACValues[PITCH] = 245; |
||
| 1612 | dongfang | 45 | } |
| 46 | if(DACValues[DAC_ROLL] < 10) { |
||
| 1645 | - | 47 | /* GyroDefectRoll = 1; */ DACValues[ROLL] = 10; |
| 1612 | dongfang | 48 | } else if(DACValues[DAC_ROLL] > 245) { |
| 1645 | - | 49 | /* GyroDefectRoll = 1; */ DACValues[ROLL] = 245; |
| 1612 | dongfang | 50 | } |
| 51 | if(DACValues[DAC_YAW] < 10) { |
||
| 52 | /* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 10; |
||
| 53 | } else if(DACValues[DAC_YAW] > 245) { |
||
| 54 | /* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 245; |
||
| 55 | } |
||
| 56 | |||
| 57 | I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
||
| 58 | |||
| 59 | // Wait for I2C to finish transmission. |
||
| 60 | while(twi_state) { |
||
| 61 | // Did it take too long? |
||
| 62 | if(CheckDelay(timeout)) { |
||
| 63 | printf("\r\n DAC or I2C Error1 check I2C, 3Vref, DAC, and BL-Ctrl"); |
||
| 64 | break; |
||
| 65 | } |
||
| 66 | } |
||
| 67 | |||
| 68 | analog_start(); |
||
| 69 | |||
| 70 | Delay_ms_Mess(i<10 ? 10 : 2); |
||
| 71 | } |
||
| 72 | Delay_ms_Mess(70); |
||
| 73 | } |
||
| 74 | |||
| 75 | void gyro_setDefaults(void) { |
||
| 76 | staticParams.GyroD = 3; |
||
| 77 | staticParams.DriftComp = 32; |
||
| 78 | staticParams.GyroAccFactor = 5; |
||
| 79 | |||
| 80 | // Not used. |
||
| 81 | staticParams.AngleTurnOverPitch = 85; |
||
| 82 | staticParams.AngleTurnOverRoll = 85; |
||
| 83 | } |