How do castes evolve in eusocial species?
Does a fertile queen have the code for soldiers/workers in its own genes, so only a mutation to the queen results in change of the whole coloniy's castes?


Yes.


  But how that would protect them from just going extinct due to one unfavorable mutation to a queen?


One unfavorable mutation to a queen could make that queen and her lineage "extinct". It wouldn't necessarily have any effect on the rest of the population (i.e. other queens and their lineages).  There's no difference here between eusocial and other species.