50ms ist ja massig Zeit, wenn man nicht gut aufpasst wird einem in 50ms ja schon die Sahne sauer...

Kommt drauf an, wie viel du in einen 50ms Frame reinpackst. Ich denke mal ein 128 sollte das locker packen. Der Übelstand ist, daß du IRQs nicht priorisieren kannst. Ich vermute mal die 50ms-Teile segeln über den UART.

Ich selbst hab noch nie mit Servos gearbeitet weiß also nicht, wie groß der Jitter auf ihrem Signal sein darf und ab wann sie sich verschlucken und zappeln.

Den ADXL liest du wahrscheinlich besser via InCapt, das gibt genaue Werte. Das ISR-Design kannst du auf 2 Ebenen machen: Eine Ebene ist offen für IRQs, die andere (höher priore) ist es nicht.

Wenn du direkt an die Ports gehst macht du es so: Du berechnest erst alle Ausgaben und setzt sie dann (quasi) parallel, anstatt berechnen-setzen-berechnen-... Zudem muss der µC genug Strom für die Servos liefern. Aber 120mA oder so sollten für 32*TTL reichen.

Evtl ist auch zu überlegen, in deiner Applikation auf die 50ms zu pollen. (Ein Programm hat niemals nicht irgendwo Warteschleifen!). Musst eben mal gegeneinander aufrechen, was wie lange warten kann/darf:
-- 50ms-Baudrate
-- min/max Duty des ADXL
-- Jitter auf Servo-OUT