Rev 2073 | Rev 2088 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2073 | Rev 2086 | ||
---|---|---|---|
Line 40... | Line 40... | ||
40 | * integration to angles. |
40 | * integration to angles. |
41 | */ |
41 | */ |
42 | int16_t gyro_PID[2]; |
42 | int16_t gyro_PID[2]; |
43 | int16_t gyro_ATT[2]; |
43 | int16_t gyro_ATT[2]; |
44 | int16_t gyroD[2]; |
44 | int16_t gyroD[2]; |
- | 45 | int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH]; |
|
- | 46 | uint8_t gyroDWindowIdx = 0; |
|
45 | int16_t yawGyro; |
47 | int16_t yawGyro; |
46 | int16_t magneticHeading; |
48 | int16_t magneticHeading; |
Line 47... | Line 49... | ||
47 | 49 | ||
- | 50 | int32_t groundPressure; |
|
Line 48... | Line 51... | ||
48 | int32_t groundPressure; |
51 | int16_t dHeight; |
49 | 52 | ||
50 | /* |
53 | /* |
51 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
54 | * Offset values. These are the raw gyro and acc. meter sums when the copter is |
Line 358... | Line 361... | ||
358 | 361 | ||
359 | // 4) Measure noise. |
362 | // 4) Measure noise. |
Line 360... | Line 363... | ||
360 | measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
363 | measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
361 | 364 | ||
- | 365 | // 5) Differential measurement. |
|
- | 366 | // gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant; |
|
- | 367 | int16_t diff = tempOffsetGyro[axis] - gyro_PID[axis]; |
|
- | 368 | gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx]; |
|
Line 362... | Line 369... | ||
362 | // 5) Differential measurement. |
369 | gyroD[axis] += diff; |
363 | gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant; |
370 | gyroDWindow[axis][gyroDWindowIdx] = diff; |
Line 364... | Line 371... | ||
364 | 371 | ||
365 | // 6) Done. |
372 | // 6) Done. |
366 | gyro_PID[axis] = tempOffsetGyro[axis]; |
373 | gyro_PID[axis] = tempOffsetGyro[axis]; |
367 | 374 | ||
- | 375 | // Prepare tempOffsetGyro for next calculation below... |
|
- | 376 | tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
|
- | 377 | } |
|
- | 378 | ||
368 | // Prepare tempOffsetGyro for next calculation below... |
379 | if (gyroDWindowIdx >= GYRO_D_WINDOW_LENGTH) { |
369 | tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
380 | gyroDWindowIdx = 0; |
370 | } |
381 | } |
371 | 382 | ||
Line 490... | Line 501... | ||
490 | airPressureWindow[windowPtr++] = simpleAirPressure; |
501 | airPressureWindow[windowPtr++] = simpleAirPressure; |
491 | if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0; |
502 | if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0; |
492 | filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength; |
503 | filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength; |
493 | } |
504 | } |
Line -... | Line 505... | ||
- | 505 | ||
494 | 506 | // positive diff of pressure |
|
- | 507 | int16_t diff = filteredAirPressure - lastFilteredAirPressure; |
|
- | 508 | // is a negative diff of height. |
|
- | 509 | dHeight -= diff; |
|
- | 510 | // remove old sample (fifo) from window. |
|
- | 511 | dHeight += dAirPressureWindow[dWindowPtr]; |
|
495 | dAirPressureWindow[dWindowPtr++] = (int16_t)(filteredAirPressure - lastFilteredAirPressure); |
512 | dAirPressureWindow[dWindowPtr++] = diff; |
496 | if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0; |
- | |
497 | 513 | if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0; |
|
498 | pressureMeasurementCount = airPressureSum = 0; |
514 | pressureMeasurementCount = airPressureSum = 0; |
499 | } |
515 | } |
500 | } |
516 | } |
Line 530... | Line 546... | ||
530 | accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
546 | accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
531 | accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
547 | accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
532 | } |
548 | } |
Line 533... | Line 549... | ||
533 | 549 | ||
- | 550 | // Noise is relative to offset. So, reset noise measurements when changing offsets. |
|
534 | // Noise is relative to offset. So, reset noise measurements when changing offsets. |
551 | for (uint8_t i=PITCH; i<=ROLL; i++) { |
535 | gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
552 | gyroNoisePeak[i] = 0; |
- | 553 | accNoisePeak[i] = 0; |
|
- | 554 | gyroD[i] = 0; |
|
- | 555 | for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) { |
|
- | 556 | gyroDWindow[i][j] = 0; |
|
536 | accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0; |
557 | } |
537 | 558 | } |
|
538 | // Setting offset values has an influence in the analog.c ISR |
559 | // Setting offset values has an influence in the analog.c ISR |
539 | // Therefore run measurement for 100ms to achive stable readings |
560 | // Therefore run measurement for 100ms to achive stable readings |
Line 540... | Line 561... | ||
540 | delay_ms_with_adc_measurement(100, 0); |
561 | delay_ms_with_adc_measurement(100, 0); |
Line 621... | Line 642... | ||
621 | int32_t analog_getHeight(void) { |
642 | int32_t analog_getHeight(void) { |
622 | return groundPressure - filteredAirPressure; |
643 | return groundPressure - filteredAirPressure; |
623 | } |
644 | } |
Line 624... | Line 645... | ||
624 | 645 | ||
625 | int16_t analog_getDHeight(void) { |
- | |
626 | int16_t result = 0; |
- | |
627 | for (int i=0; i<staticParams.airpressureDWindowLength; i++) { |
- | |
628 | result -= dAirPressureWindow[i]; // minus pressure is plus height. |
- | |
629 | } |
- | |
630 | - | ||
631 | debugOut.analog[30] = result; |
- | |
632 | // dHeight = -dPressure, so here it is the old pressure minus the current, not opposite. |
646 | int16_t analog_getDHeight(void) { |
633 | return result; |
647 | return dHeight; |