Add waipoints when changing ascend rate in planner

Signed-off-by: Robert C. Helling <helling@atdotde.de>