FIx compilation error when enabling SERVO_ENDSTOPS (#591)