PRobot,

prima ! Beim Durchsehen des Algorithmus (mit beschränkten C-Kenntnissen) ist mir nur aufgefallen, dass Du e_q_norm_y mit 0.0 initialisierst; der Startwert muss aber 1.0 sein.

Auf die Variablen e_q_norm_x und e_q_norm_y kannst Du ganz verzichten, wenn Du schreibst

double e_p_not_norm_x = (*Last_e_p_norm_x) - delta_angle*(*Last_e_p_norm_y)
double e_p_not_norm_y = (*Last_e_p_norm_y) + delta_angle*(*Last_e_p_norm_x)

Das ist eine mögliche, aber keine notwendige Vereinfachung des Programms.

Freut mich, dass Du's hingekriegt hast. Berichte 'mal über die Testfahrten!

Ciao,

mare_crisium

Edit: Fehler in Formel für e_p_not_norm_y korrigiert: Zeiger (*Last_e_p_norm_x) durch Zeiger (*Last_e_p_norm_y) ersetzt (Danke, für den Hinweis, SternThaler !