| 8,62 → 8,68 |
|---|
| #define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510 |
| #define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515 |
| |
| const uint8_t GYRO_REVERSED[3] = {0,0,1}; |
| const uint8_t ACC_REVERSED[3] = {0,1,0}; |
| const uint8_t GYRO_REVERSED[3] = { 0, 0, 1 }; |
| const uint8_t ACC_REVERSED[3] = { 0, 1, 0 }; |
| |
| // void gyro_init(void) {} |
| void gyro_calibrate(void) { |
| uint8_t i, axis, factor, numberOfAxesInRange = 0; |
| uint16_t timeout; |
| // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
| timeout = SetDelay(2000); |
| uint8_t i, axis, factor, numberOfAxesInRange = 0; |
| uint16_t timeout; |
| // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
| timeout = SetDelay(2000); |
| |
| for(i = 140; i != 0; i--) { |
| // If all 3 axis are in range, shorten the remaining number of iterations. |
| if(numberOfAxesInRange == 3 && i > 10) i = 9; |
| numberOfAxesInRange = 0; |
| |
| for (axis=PITCH; axis<=YAW; axis++) { |
| if (axis==YAW) factor = GYRO_SUMMATION_FACTOR_YAW; |
| else factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
| |
| if(rawGyroSum[axis] < 510 * factor) DACValues[axis]--; |
| else if(rawGyroSum[axis] > 515 * factor) DACValues[axis]++; |
| else numberOfAxesInRange++; |
| |
| /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
| if(DACValues[axis] < 10) { |
| DACValues[axis] = 10; |
| } else if(DACValues[axis] > 245) { |
| DACValues[axis] = 245; |
| for (i = 140; i != 0; i--) { |
| // If all 3 axis are in range, shorten the remaining number of iterations. |
| if (numberOfAxesInRange == 3 && i > 10) |
| i = 9; |
| numberOfAxesInRange = 0; |
| |
| for (axis = PITCH; axis <= YAW; axis++) { |
| if (axis == YAW) |
| factor = GYRO_SUMMATION_FACTOR_YAW; |
| else |
| factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
| |
| if (rawGyroSum[axis] < 510 * factor) |
| DACValues[axis]--; |
| else if (rawGyroSum[axis] > 515 * factor) |
| DACValues[axis]++; |
| else |
| numberOfAxesInRange++; |
| |
| /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
| if (DACValues[axis] < 10) { |
| DACValues[axis] = 10; |
| } else if (DACValues[axis] > 245) { |
| DACValues[axis] = 245; |
| } |
| } |
| |
| I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
| |
| // Wait for I2C to finish transmission. |
| while (twi_state) { |
| // Did it take too long? |
| if (CheckDelay(timeout)) { |
| printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
| break; |
| } |
| } |
| |
| analog_start(); |
| |
| Delay_ms_Mess(i < 10 ? 10 : 2); |
| } |
| } |
| |
| I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
| |
| // Wait for I2C to finish transmission. |
| while(twi_state) { |
| // Did it take too long? |
| if(CheckDelay(timeout)) { |
| printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
| break; |
| } |
| } |
| |
| analog_start(); |
| |
| Delay_ms_Mess(i<10 ? 10 : 2); |
| } |
| Delay_ms_Mess(70); |
| Delay_ms_Mess(70); |
| } |
| |
| void gyro_setDefaults(void) { |
| staticParams.GyroD = 3; |
| staticParams.DriftComp = 250; |
| staticParams.GyroAccFactor = 10; |
| staticParams.GyroAccTrim = 1; |
| staticParams.GyroD = 3; |
| staticParams.DriftComp = 250; |
| staticParams.GyroAccFactor = 10; |
| staticParams.GyroAccTrim = 1; |
| |
| // Not used. |
| staticParams.AngleTurnOverPitch = 85; |
| staticParams.AngleTurnOverRoll = 85; |
| // Not used. |
| staticParams.AngleTurnOverPitch = 85; |
| staticParams.AngleTurnOverRoll = 85; |
| } |