Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2112 → Rev 2189

/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
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;
}