du brauchst keinen winkel errechnen wie klebwax schon erwähnt. ich mache selbst auhc imemr den fehler und denke so. aber der winkel ist einfach nur eine andere einheit. den weg kann man sich sparen.

Also du musst nur den mittelwert für den servo finden. so das dieser grade steht. evtl ein offset einplanen. ist das selbe prinzip wie mit den x und y offset beim hovermode.

die werte in den P_set_nick_int usw. sind nur errorwerte. diese solltets du nicht direkt aufs servo geben.