waypoint fix

Signed-off-by: Thibaud ARNAULT <thibaud.arnault@gmail.com>