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];
|
PTermGYRO = rcCommand[axis];
|
||||||
|
|
||||||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
|
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;
|
errorGyroI[axis] = 0;
|
||||||
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
|
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
|
||||||
}
|
}
|
||||||
|
@ -370,7 +370,7 @@ static void pidRewrite(void)
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (axis = 0; axis < 3; axis++) {
|
||||||
// -----Get the desired angle rate depending on flight mode
|
// -----Get the desired angle rate depending on flight mode
|
||||||
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
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 {
|
} else {
|
||||||
// calculate error and limit the angle to 50 degrees max inclination
|
// 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
|
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