Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1645 → Rev 1646

/branches/dongfang_FC_rewrite/analog.c
72,9 → 72,9
* reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
* the offsets with the DAC.
*/
volatile int16_t rawGyroSum[2], rawYawGyroSum;
volatile int16_t acc[2] = {0,0}, ZAcc = 0;
volatile int16_t filteredAcc[2] = {0,0};
volatile int16_t rawGyroSum[3];
volatile int16_t acc[3];
volatile int16_t filteredAcc[2]={0,0};
 
/*
* These 4 exported variables are zero-offset. The "PID" ones are used
84,7 → 84,7
volatile int16_t gyro_PID[2];
volatile int16_t gyro_ATT[2];
volatile int16_t gyroD[2];
volatile int16_t yawGyro = 0;
volatile int16_t yawGyro;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
91,16 → 91,25
* standing still. They are used for adjusting the gyro and acc. meter values
* to be centered on zero.
*/
volatile int16_t gyroOffset[2], yawGyroOffset;
volatile int16_t accOffset[2], ZAccOffset;
volatile int16_t gyroOffset[3] = {
512 * GYRO_SUMMATION_FACTOR_PITCHROLL,
512 * GYRO_SUMMATION_FACTOR_PITCHROLL,
512 * GYRO_SUMMATION_FACTOR_YAW
};
 
volatile int16_t accOffset[3] = {
512 * ACC_SUMMATION_FACTOR_PITCHROLL,
512 * ACC_SUMMATION_FACTOR_PITCHROLL,
512 * ACC_SUMMATION_FACTOR_Z
};
 
/*
* This allows some experimentation with the gyro filters.
* Should be replaced by #define's later on...
*/
volatile uint8_t GYROS_FIRSTORDERFILTER;
volatile uint8_t GYROS_SECONDORDERFILTER;
volatile uint8_t GYROS_DFILTER;
volatile uint8_t GYROS_PID_FILTER;
volatile uint8_t GYROS_ATT_FILTER;
volatile uint8_t GYROS_D_FILTER;
volatile uint8_t ACC_FILTER;
 
/*
253,39 → 262,35
*/
switch(state++) {
case 7: // Z acc
#ifdef ACC_REVERSE_ZAXIS
ZAcc = -ZAccOffset - sensorInputs[AD_ACC_Z];
#else
ZAcc = sensorInputs[AD_ACC_Z] - ZAccOffset;
#endif
break;
if (ACC_REVERSED[Z])
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
break;
case 10: // yaw gyro
rawYawGyroSum = sensorInputs[AD_GYRO_YAW];
#ifdef GYRO_REVERSE_YAW
yawGyro = rawYawGyroSum - yawGyroOffset;
#else
yawGyro = yawGyroOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3).
#endif
rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
if (GYRO_REVERSED[YAW])
yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
else
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
break;
case 11: // pitch axis acc.
#ifdef ACC_REVERSE_PITCHAXIS
acc[PITCH] = -accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
#else
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
#endif
if (ACC_REVERSED[PITCH])
acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
else
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
 
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;
 
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
break;
case 12: // roll axis acc.
#ifdef ACC_REVERSE_ROLLAXIS
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
#else
acc[ROLL] = -accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
#endif
if (ACC_REVERSED[ROLL])
acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
else
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER;
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);
break;
314,9 → 319,9
filteredAirPressure = filterAirPressure(getAbsPressure(rawAirPressure));
}
DebugOut.Analog[12] = range;
DebugOut.Analog[13] = rawAirPressure;
DebugOut.Analog[14] = filteredAirPressure;
DebugOut.Analog[13] = range;
DebugOut.Analog[14] = rawAirPressure;
DebugOut.Analog[15] = filteredAirPressure;
break;
 
case 14:
340,7 → 345,7
}
 
// 2) Apply sign and offset, scale before filtering.
if (GYROS_REVERSE[axis]) {
if (GYRO_REVERSED[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
347,13 → 352,13
}
 
// 3) Scale and filter.
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PIDFILTER-1) + tempOffsetGyro) / GYROS_PIDFILTER;
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER-1) + tempOffsetGyro) / GYROS_PID_FILTER;
 
// 4) Measure noise.
measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
 
// 5) Differential measurement.
gyroD[axis] = (gyroD[axis] * (GYROS_DFILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_DFILTER;
gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER;
 
// 6) Done.
gyro_PID[axis] = tempOffsetGyro;
364,7 → 369,7
tempGyro = rawGyroSum[axis];
// 1) Apply sign and offset, scale before filtering.
if (GYROS_REVERSE[axis]) {
if (GYRO_REVERSED[axis]) {
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
} else {
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
371,7 → 376,7
}
// 2) Filter.
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_INTEGRALFILTER-1) + tempOffsetGyro) / GYROS_INTEGRALFILTER;
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER-1) + tempOffsetGyro) / GYROS_ATT_FILTER;
break;
case 16:
400,41 → 405,39
 
void analog_calibrate(void) {
#define GYRO_OFFSET_CYCLES 32
uint8_t i;
int32_t _pitchOffset = 0, _rollOffset = 0, _yawOffset = 0;
uint8_t i, axis;
int32_t deltaOffsets[3] = {0,0,0};
int16_t filteredDelta;
 
// Set the filters... to be removed again, once some good settings are found.
GYROS_FIRSTORDERFILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1;
GYROS_SECONDORDERFILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
GYROS_DFILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1;
GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
 
gyroOffset[PITCH] = gyroOffset[ROLL] = yawGyroOffset = 0;
 
gyro_calibrate();
 
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for(i=0; i < GYRO_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
_pitchOffset += rawGyroSum[PITCH];
_rollOffset += rawGyroSum[ROLL];
_yawOffset += rawYawGyroSum;
for (axis=0; axis<=YAW; axis++) {
deltaOffsets[axis] += rawGyroSum[axis] - gyroOffset[axis];
}
}
gyroOffset[PITCH] = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
gyroOffset[ROLL] = (_rollOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
yawGyroOffset = (_yawOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
gyro_PID[PITCH] = gyro_PID[ROLL] = 0;
gyro_ATT[PITCH] = gyro_ATT[ROLL] = 0;
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
 
for (axis=0; axis<=YAW; axis++) {
filteredDelta = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
gyroOffset[axis] += filteredDelta;
}
 
// Noise is relative to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
 
accOffset[PITCH] = (int16_t)GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = (int16_t)GetParamWord(PID_ACC_ROLL);
ZAccOffset = (int16_t)GetParamWord(PID_ACC_TOP);
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = GetParamWord(PID_ACC_ROLL);
accOffset[Z] = GetParamWord(PID_ACC_Z);
 
Delay_ms_Mess(100);
}
 
/*
446,30 → 449,35
*/
void analog_calibrateAcc(void) {
#define ACC_OFFSET_CYCLES 10
uint8_t i;
int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0;
uint8_t i, axis;
int32_t deltaOffset[3] = {0,0,0};
int16_t filteredDelta;
// int16_t pressureDiff, savedRawAirPressure;
accOffset[PITCH] = accOffset[ROLL] = ZAccOffset = 0;
 
for(i=0; i < ACC_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
_pitchAxisOffset += acc[PITCH];
_rollAxisOffset += acc[ROLL];
_ZAxisOffset += ZAcc;
for (axis=0; axis<=YAW; axis++) {
deltaOffset[axis] += acc[axis];
}
}
 
for (axis=0; axis<=YAW; axis++) {
filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
}
 
// Save ACC neutral settings to eeprom
SetParamWord(PID_ACC_PITCH, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_ROLL, (uint16_t)((_rollAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_TOP, (uint16_t)((_ZAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_PITCH, accOffset[PITCH]);
SetParamWord(PID_ACC_ROLL, accOffset[ROLL]);
SetParamWord(PID_ACC_Z, accOffset[Z]);
 
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
// Delay_ms_Mess(100);
Delay_ms_Mess(100);
// Set the feedback so that air pressure ends up in the middle of the range.
// (raw pressure high --> OCR0A also high...)
495,7 → 503,7
pressureDiff += (rawAirPressure - savedRawAirPressure);
}
 
DebugOut.Analog[15] = rangewidth =
DebugOut.Analog[16] = rangewidth =
(pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2);
*/
}