Fix problems in servo output