Check waypoint timing. Hydro backport

Hydro backport of #145.