Add servo on command and set SV bit on home command