139,7 → 139,6 |
} |
|
/************************************************************************/ |
|
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
154,7 → 153,9 |
PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING) |
+ (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
|
|
/************************************************************************/ |
/* Stick signals are positive and gyros are negative... */ |
/************************************************************************/ |
IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH]; |
if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360; |
else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360; |