100,14 → 100,7 |
* standing still. They are used for adjusting the gyro and acc. meter values |
* to be centered on zero. |
*/ |
/* |
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 }; |
*/ |
|
sensorOffset_t gyroOffset; |
sensorOffset_t accOffset; |
|
474,7 → 467,31 |
analog_updateBatteryVoltage(); |
} |
|
void analog_calibrate(void) { |
void analog_setNeutral() { |
if (gyroOffset_readFromEEProm()) { |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
} |
|
if (accOffset_readFromEEProm()) { |
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL; |
accOffset.offsets[Z] = 512 * ACC_SUMMATION_FACTOR_Z; |
} |
|
// Noise is relative to offset. So, reset noise measurements when changing offsets. |
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
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); |
|
// Rough estimate. Hmm no nothing happens at calibration anyway. |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
// pressureMeasurementCount = 0; |
} |
|
void analog_calibrateGyros(void) { |
#define GYRO_OFFSET_CYCLES 32 |
uint8_t i, axis; |
int32_t deltaOffsets[3] = { 0, 0, 0 }; |
492,20 → 509,8 |
gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
debugOut.analog[6+axis] = gyroOffset.offsets[axis]; |
} |
|
// Noise is relativ to offset. So, reset noise measurements when changing offsets. |
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
|
accOffset_readFromEEProm(); |
// accOffset[PITCH] = getParamWord(PID_ACC_PITCH); |
// accOffset[ROLL] = getParamWord(PID_ACC_ROLL); |
// accOffset[Z] = getParamWord(PID_ACC_Z); |
|
// Rough estimate. Hmm no nothing happens at calibration anyway. |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
// pressureMeasurementCount = 0; |
|
delay_ms_Mess(100); |
|
gyroOffset_writeToEEProm(); |
} |
|
/* |
534,18 → 539,6 |
/ ACC_OFFSET_CYCLES; |
accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
} |
|
// Save ACC neutral settings to eeprom |
// setParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
// setParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
// setParamWord(PID_ACC_Z, accOffset[Z]); |
|
accOffset_writeToEEProm(); |
|
// 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); |
} |