"All units share same GND!"

-> Tu dir das nicht an. Galvanische Trennung vom Rechen- und Leistungsteil ist das A und O.
Also Optokoppler zwischen Controller und Servos und eigene Stromversorgung.

Du solltest auch überlegen, ob es sinnvoll ist irgendwelche Sensoren die 10mA verbrauchen in den Stromsparmodus zu setzen, wenn dein bein durch die Servos im Ruhezustand schon 500mA frisst.