Remove unused FixYaw. Add new waypoint system. Test mission: follow
waypoints (square) and take a picture at each