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) |