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