| 42,10 → 42,13 |
|---|
| int16_t gyro_PID[2]; |
| int16_t gyro_ATT[2]; |
| int16_t gyroD[2]; |
| int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH]; |
| uint8_t gyroDWindowIdx = 0; |
| int16_t yawGyro; |
| int16_t magneticHeading; |
| |
| int32_t groundPressure; |
| int16_t dHeight; |
| |
| /* |
| * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| 360,7 → 363,11 |
|---|
| measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
| |
| // 5) Differential measurement. |
| gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant; |
| // gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant; |
| int16_t diff = tempOffsetGyro[axis] - gyro_PID[axis]; |
| gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx]; |
| gyroD[axis] += diff; |
| gyroDWindow[axis][gyroDWindowIdx] = diff; |
| |
| // 6) Done. |
| gyro_PID[axis] = tempOffsetGyro[axis]; |
| 368,7 → 375,11 |
|---|
| // Prepare tempOffsetGyro for next calculation below... |
| tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
| } |
| |
| |
| if (gyroDWindowIdx >= GYRO_D_WINDOW_LENGTH) { |
| gyroDWindowIdx = 0; |
| } |
| |
| /* |
| * Now process the data for attitude angles. |
| */ |
| 492,9 → 503,14 |
|---|
| filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength; |
| } |
| |
| dAirPressureWindow[dWindowPtr++] = (int16_t)(filteredAirPressure - lastFilteredAirPressure); |
| // positive diff of pressure |
| int16_t diff = filteredAirPressure - lastFilteredAirPressure; |
| // is a negative diff of height. |
| dHeight -= diff; |
| // remove old sample (fifo) from window. |
| dHeight += dAirPressureWindow[dWindowPtr]; |
| dAirPressureWindow[dWindowPtr++] = diff; |
| if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0; |
| |
| pressureMeasurementCount = airPressureSum = 0; |
| } |
| } |
| 532,9 → 548,14 |
|---|
| } |
| |
| // Noise is relative to offset. So, reset noise measurements when changing offsets. |
| gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
| accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
| |
| for (uint8_t i=PITCH; i<=ROLL; i++) { |
| gyroNoisePeak[i] = 0; |
| accNoisePeak[i] = 0; |
| gyroD[i] = 0; |
| for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) { |
| gyroDWindow[i][j] = 0; |
| } |
| } |
| // Setting offset values has an influence in the analog.c ISR |
| // Therefore run measurement for 100ms to achive stable readings |
| delay_ms_with_adc_measurement(100, 0); |
| 623,12 → 644,5 |
|---|
| } |
| |
| int16_t analog_getDHeight(void) { |
| int16_t result = 0; |
| for (int i=0; i<staticParams.airpressureDWindowLength; i++) { |
| result -= dAirPressureWindow[i]; // minus pressure is plus height. |
| } |
| |
| debugOut.analog[30] = result; |
| // dHeight = -dPressure, so here it is the old pressure minus the current, not opposite. |
| return result; |
| return dHeight; |
| } |