0,0 → 1,82 |
//#include "ENC-03_FC1.3.h" |
#include "printf_P.h" |
#include "analog.h" |
#include "twimaster.h" |
#include "configuration.h" |
#include "timer0.h" |
|
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510 |
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515 |
|
#define DAC_PITCH 0 |
#define DAC_ROLL 1 |
#define DAC_YAW 2 |
|
// void gyro_init(void) {} |
void gyro_calibrate(void) { |
uint8_t i, 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; |
|
if(rawPitchGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_PITCH]--; |
else if(rawPitchGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_PITCH]++; |
else numberOfAxesInRange++; |
|
if(rawRollGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_ROLL]--; |
else if(rawRollGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_ROLL]++; |
else numberOfAxesInRange++; |
|
if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--; |
else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ; |
else numberOfAxesInRange++; |
|
if(DACValues[DAC_PITCH] < 10) { |
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 10; |
} else if(DACValues[DAC_PITCH] > 245) { |
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 245; |
} |
if(DACValues[DAC_ROLL] < 10) { |
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 10; |
} else if(DACValues[DAC_ROLL] > 245) { |
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 245; |
} |
if(DACValues[DAC_YAW] < 10) { |
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 10; |
} else if(DACValues[DAC_YAW] > 245) { |
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 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 Error1 check I2C, 3Vref, DAC, and BL-Ctrl"); |
break; |
} |
} |
|
analog_start(); |
|
Delay_ms_Mess(i<10 ? 10 : 2); |
} |
Delay_ms_Mess(70); |
} |
|
void gyro_setDefaults(void) { |
staticParams.GyroD = 3; |
staticParams.DriftComp = 32; |
staticParams.GyroAccFactor = 5; |
|
// Not used. |
staticParams.AngleTurnOverPitch = 85; |
staticParams.AngleTurnOverRoll = 85; |
} |