Feature/read waypoint file

Also checking if waypoint sent correctly.