1,11 → 1,13 |
#include "sensors.h" |
#include "printf_P.h" |
#include "analog.h" |
#include "twimaster.h" |
#include "configuration.h" |
#include "eeprom.h" |
#include "timer0.h" |
#include "output.h" |
|
#include <stdio.h> |
|
void I2C_OutputAmplifierOffsets(void) { |
uint16_t timeout = setDelay(2000); |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
19,39 → 21,50 |
} |
} |
|
uint8_t DACChannelFor(uint8_t axis) { |
switch (axis) { |
case X: |
return 1; |
case Y: |
return 0; |
case Z: |
return 2; |
default: |
return -1; // should never happen. |
} |
} |
|
void gyro_calibrate(void) { |
printf("gyro_calibrate"); |
uint8_t i, axis, factor, numberOfAxesInRange = 0; |
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
|
for (i = 140; i != 0; i--) { |
delay_ms_with_adc_measurement(i <= 10 ? 10 : 2, 1); |
|
|
for (i = 200; i != 0; i--) { |
waitADCCycle(i <= 10 ? 10 : 2); |
// 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++) { |
if (axis == YAW) |
factor = GYRO_OVERSAMPLING_YAW; |
for (axis=X; axis<=Z; axis++) { |
uint8_t dacChannel = DACChannelFor(axis); |
if (axis==Z) |
factor=GYRO_OVERSAMPLING_Z; |
else |
factor = GYRO_OVERSAMPLING_PITCHROLL; |
factor=GYRO_OVERSAMPLING_XY; |
|
if (rawGyroValue(axis) < 510 * factor) |
gyroAmplifierOffset.offsets[axis]--; |
else if (rawGyroValue(axis) > 515 * factor) |
gyroAmplifierOffset.offsets[axis]++; |
if (gyroValueForFC13DACCalibration(axis) < (uint16_t)(510*factor)) |
gyroAmplifierOffset.offsets[dacChannel]--; |
else if (gyroValueForFC13DACCalibration(axis) > (uint16_t)(515 * factor)) |
gyroAmplifierOffset.offsets[dacChannel]++; |
else |
numberOfAxesInRange++; |
|
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
if (gyroAmplifierOffset.offsets[axis] < 10) { |
gyroAmplifierOffset.offsets[axis] = 10; |
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH << axis); |
} else if (gyroAmplifierOffset.offsets[axis] > 245) { |
gyroAmplifierOffset.offsets[axis] = 245; |
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH << axis); |
if (gyroAmplifierOffset.offsets[dacChannel] < 10) { |
gyroAmplifierOffset.offsets[dacChannel] = 10; |
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis); |
} else if (gyroAmplifierOffset.offsets[dacChannel] > 245) { |
gyroAmplifierOffset.offsets[dacChannel] = 245; |
versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis); |
} |
} |
|
58,15 → 71,15 |
I2C_OutputAmplifierOffsets(); |
} |
gyroAmplifierOffset_writeToEEProm(); |
delay_ms_with_adc_measurement(70, 0); |
waitADCCycle(70); |
} |
|
void gyro_init(void) { |
if (gyroAmplifierOffset_readFromEEProm()) { |
printf("gyro amp invalid, recalibrate."); |
gyroAmplifierOffset.offsets[PITCH] = |
gyroAmplifierOffset.offsets[ROLL] = |
gyroAmplifierOffset.offsets[YAW] = (uint8_t)(255 * 1.2089 / 3.0); |
gyroAmplifierOffset.offsets[X] = |
gyroAmplifierOffset.offsets[Y] = |
gyroAmplifierOffset.offsets[Z] = (uint8_t)(255 * 1.2089 / 3.0); |
} else { |
I2C_OutputAmplifierOffsets(); |
} |
73,11 → 86,8 |
} |
|
void gyro_setDefaultParameters(void) { |
IMUConfig.gyroQuadrant = 0; |
IMUConfig.gyroQuadrant = 4; |
IMUConfig.accQuadrant = 4; |
IMUConfig.imuReversedFlags = IMU_REVERSE_GYRO_YAW | IMU_REVERSE_ACC_XY; |
IMUConfig.imuReversedFlags = IMU_REVERSE_ACCEL_Z | IMU_REVERSE_GYRO_Z; |
staticParams.gyroD = 3; |
IMUConfig.driftCompDivider = 1; |
IMUConfig.driftCompLimit = 200; |
IMUConfig.zerothOrderCorrection = 120; |
} |