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