Sync some code change and fix from Prusa. Thanks Prusa. This also can fix github issue #317 Signed-off-by: salt.wei <salt.wei@bambulab.com> Change-Id: If6993e0149733ccf85ed85f82553caa03df7ac60