There is simply NO solution for this in SH5.
All you can have is what is done in "UHS" mod - draft is set to 5,45 instead of 4,5 = sub does not fly as much as when stock setting used.
Believe me, ppl tried to tweak this for weeks (including me) so unless ubi does not change their wave engine/sub mass algorythm, it will always look like it moves on rails.
Or, of course, you can do what we done here 100 times - increase submerged mass/displacement and lower up/down drag for surfaced "obj_hydro".
I`ve had a little success when editing "sim.cfg":
---------------------------
[Mech]
Waves amplitude=0.3
Waves attenuation=0.75
---------------------------
These values govern how waves affect surcafe behavior too, but also detecting times for AI ships etc. so tweak this with caution.
I`ve also tried to tweak "submerged draught" to 7,5m (6 was stock I think) -> it also helped a bit so ship pitched up and down the waves but it also slowed surfacing procedure - sub stopped to surface at 7,5M and it was surfacing only very slowly up to 5,45m which is surface draft (about 2 min until fully surfaced when not using "blow ballast"- if you do then you surfaced faster)
Try it if you want its easily done and you`ll understand what i meant
