96,7 → 96,7 |
HC_periodicTask(); |
int16_t throttle = PRTY[CONTROL_THROTTLE]; |
int32_t height = analog_getHeight(); |
int32_t heightError = /*ramped*/ targetHeight - height; |
int32_t heightError = targetHeight - height; |
int16_t dHeight = analog_getDHeight(); |
|
debugOut.analog[22] = height/10L; |
114,27 → 114,37 |
debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
} |
|
int32_t heightErrorForIntegral = heightError; |
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10; |
|
if (heightErrorForIntegral > heightErrorForIntegralLimit) { |
heightErrorForIntegral = heightErrorForIntegralLimit; |
} else if (heightErrorForIntegral < -heightErrorForIntegralLimit) { |
heightErrorForIntegral =- heightErrorForIntegralLimit; |
} |
|
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
iHeight += heightError; |
iHeight += heightErrorForIntegral; |
|
#define IHEIGHT_SCALE 24 |
// dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE); |
|
if (dThrottleI > staticParams.heightControlMaxIntegral) { |
dThrottleI = staticParams.heightControlMaxIntegral; |
iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
} else if (dThrottleI < -staticParams.heightControlMaxIntegral) { |
dThrottleI = -staticParams.heightControlMaxIntegral; |
iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
if (dThrottleI > staticParams.heightControlMaxIntegralOut) { |
dThrottleI = staticParams.heightControlMaxIntegralOut; |
iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
} else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) { |
dThrottleI = -staticParams.heightControlMaxIntegralOut; |
iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
} |
|
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[10] = dThrottleP; |
debugOut.analog[11] = dThrottleI; |
debugOut.analog[12] = dThrottleD; |
debugOut.analog[13] = heightError/10; |
|
//debugOut.analog[27] = dynamicParams.heightP; |
//debugOut.analog[28] = dynamicParams.heightI; |