Add setStageSetpointCmd
Bump up default feedback frequence