86,7 → 86,7 |
volatile uint16_t sensorInputs[8]; |
volatile int16_t rawGyroSum[3]; |
volatile int16_t acc[3]; |
volatile int16_t filteredAcc[2] = { 0,0 }; |
volatile int16_t filteredAcc[3] = { 0,0,0 }; |
// volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 }; |
|
/* |
150,8 → 150,8 |
/* |
* Experiment: Measuring vibration-induced sensor noise. |
*/ |
volatile uint16_t gyroNoisePeak[2]; |
volatile uint16_t accNoisePeak[2]; |
volatile uint16_t gyroNoisePeak[3]; |
volatile uint16_t accNoisePeak[3]; |
|
// ADC channels |
#define AD_GYRO_YAW 0 |
347,6 → 347,12 |
} |
|
void analog_updateAccelerometers(void) { |
// Z acc. |
if (ACC_REVERSED[Z]) |
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
|
// Pitch and roll axis accelerations. |
for (uint8_t axis=0; axis<2; axis++) { |
if (ACC_REVERSED[axis]) |
353,27 → 359,12 |
acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
else |
acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis]; |
|
} |
|
for (uint8_t axis=0; axis<3; axis++) { |
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
|
/* |
stronglyFilteredAcc[PITCH] = |
(stronglyFilteredAcc[PITCH] * 99 + acc[PITCH] * 10) / 100; |
*/ |
|
measureNoise(acc[axis], &accNoisePeak[axis], 1); |
} |
|
// Z acc. |
if (ACC_REVERSED[Z]) |
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
|
/* |
stronglyFilteredAcc[Z] = |
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
*/ |
} |
|
void analog_updateAirPressure(void) { |
482,13 → 473,10 |
gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
} |
|
debugOut.analog[6] = gyroOffset.offsets[PITCH]; |
debugOut.analog[7] = gyroOffset.offsets[ROLL]; |
|
if (accOffset_readFromEEProm()) { |
printf("acc. meter offsets invalid%s",recal); |
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL; |
accOffset.offsets[Z] = 512 * ACC_SUMMATION_FACTOR_Z; |
accOffset.offsets[Z] = 717 * ACC_SUMMATION_FACTOR_Z; |
} |
|
// Noise is relative to offset. So, reset noise measurements when changing offsets. |