| 90,8 → 90,10 |
|---|
| int32_t height = analog_getHeight(); |
| int32_t heightError = rampedTargetHeight - height; |
| int16_t dHeight = analog_getDHeight(); |
| //lastHeight = height; |
| |
| debugOut.analog[14] = height/100L; |
| debugOut.analog[15] = dHeight; |
| |
| if (heightError > 0) { |
| debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
| } else { |
| 122,9 → 124,9 |
|---|
| int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10; |
| int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7; |
| |
| debugOut.analog[24] = dThrottleP; |
| debugOut.analog[25] = dThrottleI; |
| debugOut.analog[26] = dThrottleD; |
| //debugOut.analog[24] = dThrottleP; |
| //debugOut.analog[25] = dThrottleI; |
| //debugOut.analog[26] = dThrottleD; |
| |
| //debugOut.analog[27] = dynamicParams.heightP; |
| //debugOut.analog[28] = dynamicParams.heightI; |
| 137,11 → 139,13 |
|---|
| else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
| dThrottle = -staticParams.heightControlMaxThrottleChange; |
| |
| /* |
| debugOut.analog[19] = throttle; |
| debugOut.analog[20] = dThrottle; |
| debugOut.analog[21] = height; |
| debugOut.analog[22] = rampedTargetHeight; |
| debugOut.analog[23] = heightError; |
| */ |
| |
| if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
| if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |