Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1612 | dongfang | 1 | |
| 2 | /* |
||
| 3 | * Here is a dynamic calibration experiment: Adjust integrals and gyro offsets if the pilot appears to be always |
||
| 4 | * pushing of pulling on the pitch or roll stick. |
||
| 5 | */ |
||
| 6 | /* |
||
| 7 | if(ADCycleCount >= dynamicParams.UserParam2 * 10) { |
||
| 8 | // This algo works OK on the desk but it is a little sluggish and it oscillates. |
||
| 9 | // It does not very effectively cancel drift because of dynamics. |
||
| 10 | |||
| 11 | minStickForAutoCal = dynamicParams.UserParam3 * 10; |
||
| 12 | maxStickForAutoCal = dynamicParams.UserParam4 * 10; |
||
| 13 | |||
| 14 | // If not already corrected to the limit, and dynamic calibration is enabled: |
||
| 15 | if (abs(dynamicOffsetPitch - savedDynamicOffsetPitch) < dynamicParams.UserParam1 && !dynamicParams.UserParam6) { |
||
| 16 | // The pilot pushes on the stick, the integral is > 0, and the gyro val is > 0. Looks like a value-too-high case, so increase the offset. |
||
| 17 | if (filteredHiResPitchGyro > dynamicOffsetPitch && pitchAngle > 0 && maxStickPitch >= minStickForAutoCal && maxStickPitch <= maxStickForAutoCal) { |
||
| 18 | dynamicOffsetPitch += (int8_t)(dynamicParams.UserParam7 - 128); // (adding something seems right...) |
||
| 19 | pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
||
| 20 | } else if (filteredHiResPitchGyro < dynamicOffsetPitch && pitchAngle < 0 && maxStickPitch <= -minStickForAutoCal && maxStickPitch >= -maxStickForAutoCal) { |
||
| 21 | dynamicOffsetPitch -= (int8_t)(dynamicParams.UserParam7 - 128); // (subtracting something seems right...) |
||
| 22 | pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
||
| 23 | } |
||
| 24 | } |
||
| 25 | |||
| 26 | // If not already corrected to the limit, and dynamic calibration is enabled: |
||
| 27 | if (abs(dynamicOffsetRoll - savedDynamicOffsetRoll) <= dynamicParams.UserParam1 && !dynamicParams.UserParam6) { |
||
| 28 | if (filteredHiResRollGyro > dynamicOffsetRoll && rollAngle > 0 && maxStickRoll >= minStickForAutoCal && maxStickRoll <= maxStickForAutoCal) { |
||
| 29 | dynamicOffsetRoll += (int8_t)(dynamicParams.UserParam8 - 128); |
||
| 30 | rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
||
| 31 | } else if (filteredHiResRollGyro < dynamicOffsetRoll && rollAngle < 0 && maxStickRoll <= -minStickForAutoCal && maxStickRoll >= -maxStickForAutoCal) { |
||
| 32 | dynamicOffsetRoll -= (int8_t)(dynamicParams.UserParam8 - 128); |
||
| 33 | rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L; |
||
| 34 | } |
||
| 35 | } |
||
| 36 | ADCycleCount = 0; |
||
| 37 | } |
||
| 38 | */ |