Updated SAV MkI merge

SAV MkI merges pull request:
Tested with SAV MkI + SAV 3D LCD on i3 and Lewihe.