[guidance] use atan2f instead of INT32_ATAN for more accurate route reference

Speed and accel are saturated using route_ref angle. The calculation of route_ref was done using INT32_ATAN, which is not accurate enough to keep track on the path between 2 waypoints. Using atan2f solves this.