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); |
*/ |
} |