der rest dürfte eigentlich nicht viel zur sache tun...hoffe ihr könnt mir helfen
Da wär ich mir nicht so sicher. Im Rest stehen z.B. die Variablendeklarationen und die können falls floats dabei sind einen ganz erheblichen Einfluß auf die Laufzeit haben. Vor allem weil bei einigen Deiner Formeln Divisionen vorkommen. Im Zweifelsfall ist es eine gute Idee alles geeignet vorzuberechnen. Da bei Dir aber nichts nach PWM aussieht würde ich auch die SPI Libraries verdächtigen.