private static void SetPIDParameters(FlightComputer fc, Vector3d TfV, double kdFactor, double kpFactor, double kiFactor) { var pid = fc.pid; Vector3d invTf = TfV.InvertNoNaN(); pid.Kd = kdFactor * invTf; pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * pid.Kd; pid.Kp.Scale(invTf); pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * pid.Kp; pid.Ki.Scale(invTf); pid.intAccum = pid.intAccum.Clamp(-5, 5); }