1,4 → 1,4 |
//#include "ENC-03_FC1.3.h" |
#include "sensors.h" |
#include "printf_P.h" |
#include "analog.h" |
#include "twimaster.h" |
11,17 → 11,16 |
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) { |
void gyro_calibrate(void) { |
printf("gyro_calibrate"); |
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 = 10; |
|
numberOfAxesInRange = 0; |
|
for (axis = PITCH; axis <= YAW; axis++) { |
31,34 → 30,46 |
factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
|
if (rawGyroSum[axis] < 510 * factor) |
DACValues.offsets[axis]--; |
gyroAmplifierOffset.offsets[axis]--; |
else if (rawGyroSum[axis] > 515 * factor) |
DACValues.offsets[axis]++; |
gyroAmplifierOffset.offsets[axis]++; |
else |
numberOfAxesInRange++; |
|
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
if (DACValues.offsets[axis] < 10) { |
DACValues.offsets[axis] = 10; |
} else if (DACValues.offsets[axis] > 245) { |
DACValues.offsets[axis] = 245; |
if (gyroAmplifierOffset.offsets[axis] < 10) { |
gyroAmplifierOffset.offsets[axis] = 10; |
} else if (gyroAmplifierOffset.offsets[axis] > 245) { |
gyroAmplifierOffset.offsets[axis] = 245; |
} |
} |
|
gyro_loadOffsets(0); |
|
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
delay_ms_with_adc_measurement(i <= 10 ? 10 : 2); |
} |
gyroAmplifierOffset_writeToEEProm(); |
delay_ms_with_adc_measurement(70); |
} |
|
// 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; |
} |
} |
void gyro_loadOffsets(uint8_t overwriteWithDefaults) { |
uint16_t timeout = setDelay(2000); |
|
delay_ms_Mess(i <= 10 ? 10 : 2); |
if (overwriteWithDefaults) { |
gyroAmplifierOffset.offsets[PITCH] = |
gyroAmplifierOffset.offsets[ROLL] = |
gyroAmplifierOffset.offsets[YAW] = (uint8_t)(255 * 1.2089 / 3.0); |
} |
delay_ms_Mess(70); |
|
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; |
} |
} |
} |
|
void gyro_setDefaults(void) { |