modified updatewaypoint() but needs to be checked.