yaw improvement
this should stop the copter from carrying on spinning at high yaw rates.
This commit is contained in:
parent
51e7c71bb0
commit
20b7a9fbfb
4
src/mw.c
4
src/mw.c
|
@ -326,7 +326,7 @@ static void pidMultiWii(void)
|
|||
PTermGYRO = rcCommand[axis];
|
||||
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
|
||||
if (abs(gyroData[axis]) > 640)
|
||||
if ((abs(gyroData[axis]) > 640) || (abs(rcCommand[axis]) > 50))
|
||||
errorGyroI[axis] = 0;
|
||||
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
|
||||
}
|
||||
|
@ -370,7 +370,7 @@ static void pidRewrite(void)
|
|||
for (axis = 0; axis < 3; axis++) {
|
||||
// -----Get the desired angle rate depending on flight mode
|
||||
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
||||
AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5);
|
||||
AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[YAW]) >> 5);
|
||||
} else {
|
||||
// calculate error and limit the angle to 50 degrees max inclination
|
||||
errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]) / 10.0f; // 16 bits is ok here
|
||||
|
|
Loading…
Reference in New Issue